Subversion Repositories seema-scanner

Rev

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

Rev 44 Rev 45
Line 22... Line 22...
22 
/*
 22 
/*
23 
 * From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
 23 
 * From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
24 
 * The purpose of this function is to convert a reflected binary
 24 
 * The purpose of this function is to convert a reflected binary
25 
 * Gray code number to a binary number.
 25 
 * Gray code number to a binary number.
26 
 */
 26 
 */
27 
static unsigned grayToBinary(unsigned num, unsigned numBits)
 27 
static unsigned int grayToBinary(unsigned int num){
28 
{
 - 
 
- 
 
 28 
    unsigned int mask;
29 
    for (unsigned shift = 1; shift < numBits; shift <<= 1){
 29 
    for(mask = num >> 1; mask != 0; mask = mask >> 1)
30 
        num ^= num >> shift;
 30 
        num = num ^ mask;
31 
    }
 - 
 
32 
    return num;
 31 
    return num;
33 
}
 32 
}
34 
 
 33 
 
35 
/*
 34 
/*
36 
 * Function takes the decimal number
 - 
 
37 
 * Function takes the Nth bit (1 to 31)
 - 
 
38 
 * Return the value of Nth bit from decimal
 35 
 * Return the Nth bit of an unsigned integer number
39 
 * Source: http://icfun.blogspot.com/2009/04/get-n-th-bit-value-of-any-integer.html
 - 
 
40 
 */
 36 
 */
41 
static int get_bit(int decimal, int N){
 37 
static bool getBit(int decimal, int N){
42 
 
 38 
 
43 
    // Shifting the 1 for N-1 bits
 - 
 
44 
    int constant = 1 << (N-1);
 39 
    return decimal & 1 << (N-1);
- 
 
 40 
}
45 
 
 41 
 
46 
    // If the bit is set, return 1
 - 
 
47 
    if( decimal & constant ){
 42 
//static int get_bit(int decimal, int N){
48 
        return 1;
 - 
 
49 
    }
 - 
 
50 
 
 43 
 
- 
 
 44 
//    // Shifting the 1 for N-1 bits
- 
 
 45 
//    int constant = 1 << (N-1);
- 
 
 46 
 
51 
    // If the bit is not set, return 0
 47 
//    // If the bit is set, return 1
- 
 
 48 
//    if( decimal & constant )
- 
 
 49 
//        return 1;
- 
 
 50 
//    else
52 
    return 0;
 51 
//        return 0;
- 
 
 52 
//}
53 
}
 53 
 
- 
 
 54 
static inline unsigned int powi(int num, unsigned int exponent){
54 
 
 55 
 
55 
static inline int powi(int num, unsigned int exponent){
 - 
 
56 
    // NOT EQUIVALENT TO pow()
 - 
 
57 
    if(exponent == 0)
 56 
    if(exponent == 0)
58 
        return 1;
 57 
        return 1;
59 
 
 58 
 
60 
    float res = num;
 59 
    float res = num;
61 
    for(unsigned int i=0; i<exponent-1; i++)
 60 
    for(unsigned int i=0; i<exponent-1; i++)
62 
        res *= num;
 61 
        res *= num;
63 
 
 62 
 
64 
    return res;
 63 
    return res;
65 
}
 64 
}
66 
 
 65 
 
- 
 
 66 
static inline unsigned int twopowi(unsigned int exponent){
- 
 
 67 
 
- 
 
 68 
    return 1 << exponent;
- 
 
 69 
}
- 
 
 70 
 
67 
// Algorithm
 71 
// Algorithm
68 
AlgorithmGrayCode::AlgorithmGrayCode(unsigned int _screenCols, unsigned int _screenRows, CodingDir _dir) : Algorithm(_screenCols, _screenRows, _dir){
 72 
AlgorithmGrayCode::AlgorithmGrayCode(unsigned int _screenCols, unsigned int _screenRows, CodingDir _dir) : Algorithm(_screenCols, _screenRows, _dir){
69 
 
 73 
 
70 
    // on/off patterns
 74 
    // on/off patterns
71 
    Nbits = ceilf(log2f((float)screenCols));
 75 
    Nbits = ceilf(log2f((float)screenCols));
Line 87... Line 91...
87 
 
 91 
 
88 
        for(unsigned int j=0; j<screenCols; j++){
 92 
        for(unsigned int j=0; j<screenCols; j++){
89 
 
 93 
 
90 
            unsigned int jGray = binaryToGray(j);
 94 
            unsigned int jGray = binaryToGray(j);
91 
            // Amplitude of channels
 95 
            // Amplitude of channels
92 
            int bit = get_bit(jGray, Nbits-p);
 96 
            int bit = (int)getBit(jGray, Nbits-p);
93 
            pattern.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
 97 
            pattern.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
94 
            int invBit = bit^1;
 98 
            int invBit = bit^1;
95 
            patternInv.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
 99 
            patternInv.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
96 
        }
 100 
        }
97 
        patterns.push_back(pattern);
 101 
        patterns.push_back(pattern);
Line 109... Line 113...
109 
bool sortingLarger(cv::Vec4f i,cv::Vec4f j){ return (i[3]<j[3]);}
 113 
bool sortingLarger(cv::Vec4f i,cv::Vec4f j){ return (i[3]<j[3]);}
110 
bool sortingEqual(cv::Vec4f i,cv::Vec4f j){ return (i[3]==j[3]);}
 114 
bool sortingEqual(cv::Vec4f i,cv::Vec4f j){ return (i[3]==j[3]);}
111 
void getEdgeLabels(const cv::Mat& scanLine, int Nbits, std::vector<cv::Vec4f>& edges){
 115 
void getEdgeLabels(const cv::Mat& scanLine, int Nbits, std::vector<cv::Vec4f>& edges){
112 
 
 116 
 
113 
    int nCols = scanLine.cols;
 117 
    int nCols = scanLine.cols;
114 
    const short *data = scanLine.ptr<const short>(0);
 118 
    const int *data = scanLine.ptr<const int>(0);
115 
 
 119 
 
116 
    short labelLeft;
 120 
    int labelLeft;
117 
    short labelRight = data[0];
 121 
    int labelRight = data[0];
118 
 
 122 
 
119 
    // collect edges
 123 
    // collect edges
120 
    for(int col=1; col<nCols; col++){
 124 
    for(int col=1; col<nCols; col++){
121 
 
 125 
 
122 
        labelLeft = labelRight;
 126 
        labelLeft = labelRight;
Line 140... Line 144...
140 
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){
 144 
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){
141 
 
 145 
 
142 
    assert(frames0.size() == N);
 146 
    assert(frames0.size() == N);
143 
    assert(frames1.size() == N);
 147 
    assert(frames1.size() == N);
144 
 
 148 
 
- 
 
 149 
    for(int i=0; i<1920; i++){
- 
 
 150 
        std::cout << i << " " << binaryToGray(i) << " " << grayToBinary(binaryToGray(i)) << std::endl;
- 
 
 151 
    }
- 
 
 152 
 
- 
 
 153 
 
145 
    int frameRows = frames0[0].rows;
 154 
    int frameRows = frames0[0].rows;
146 
    int frameCols = frames0[0].cols;
 155 
    int frameCols = frames0[0].cols;
147 
 
 156 
 
148 
    // rectifying homographies (rotation+projections)
 157 
    // rectifying homographies (rotation+projections)
149 
    cv::Size frameSize(frameCols, frameRows);
 158 
    cv::Size frameSize(frameCols, frameRows);
Line 191... Line 200...
191 
    occlusion1Rect = occlusion1Rect > 50;
 200 
    occlusion1Rect = occlusion1Rect > 50;
192 
//cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");
 201 
//cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");
193 
//cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");
 202 
//cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");
194 
 
 203 
 
195 
    // decode patterns
 204 
    // decode patterns
196 
    cv::Mat code0Rect(frameRectRows, frameRectCols, CV_16S, cv::Scalar(-1));
 205 
    cv::Mat code0Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(-1));
197 
    cv::Mat code1Rect(frameRectRows, frameRectCols, CV_16S, cv::Scalar(-1));
 206 
    cv::Mat code1Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(-1));
198 
    cv::add(code0Rect, 1, code0Rect, occlusion0Rect, CV_16S);
 207 
    cv::add(code0Rect, 1, code0Rect, occlusion0Rect, CV_32S);
199 
    cv::add(code1Rect, 1, code1Rect, occlusion1Rect, CV_16S);
 208 
    cv::add(code1Rect, 1, code1Rect, occlusion1Rect, CV_32S);
200 
 
 209 
 
- 
 
 210 
    // into gray code
201 
    for(int i=0; i<Nbits; i++){
 211 
    for(int i=0; i<Nbits; i++){
202 
        cv::Mat bit0;
 212 
        cv::Mat bit0;
203 
        cv::subtract(frames0Rect[i*2+2], frames0Rect[i*2+3], bit0, cv::noArray(), CV_16S);
 213 
        cv::subtract(frames0Rect[i*2+2], frames0Rect[i*2+3], bit0);
204 
        bit0 = bit0 > 0;
 214 
        bit0 = bit0 > 0;
- 
 
 215 
        bit0.convertTo(bit0, CV_32S, 1.0/255.0);
205 
//    cvtools::writeMat(bit0, "bit0.mat", "bit0");
 216 
//      cvtools::writeMat(bit0, "bit0.mat", "bit0");
206 
        cv::add(code0Rect, bit0/255*powi(2,i), code0Rect, occlusion0Rect, CV_16S);
 217 
        cv::add(code0Rect, bit0*twopowi(Nbits-i-1), code0Rect, occlusion0Rect, CV_32S);
207 
//    cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
 - 
 
208 
        cv::Mat bit1;
 218 
        cv::Mat bit1;
209 
        cv::subtract(frames1Rect[i*2+2], frames1Rect[i*2+3], bit1, cv::noArray(), CV_16S);
 219 
        cv::subtract(frames1Rect[i*2+2], frames1Rect[i*2+3], bit1);
210 
        bit1 = bit1 > 0;
 220 
        bit1 = bit1 > 0;
- 
 
 221 
        bit1.convertTo(bit1, CV_32S, 1.0/255.0);
211 
        cv::add(code1Rect, bit1/255*powi(2,i), code1Rect, occlusion1Rect, CV_16S);
 222 
        cv::add(code1Rect, bit1*twopowi(Nbits-i-1), code1Rect, occlusion1Rect, CV_32S);
212 
    }
 223 
    }
213 
 
 224 
 
214 
//cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
 225 
//cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
215 
//cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
 226 
//cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
216 
 
 227 
 
- 
 
 228 
//    // convert to standard binary
- 
 
 229 
//    for(int r=0; r<frameRectRows; r++){
- 
 
 230 
//        for(int c=0; c<frameRectCols; c++){
- 
 
 231 
//            if(code0Rect.at<int>(r,c) != -1)
- 
 
 232 
//                code0Rect.at<int>(r,c) = grayToBinary(code0Rect.at<int>(r,c));
- 
 
 233 
//            if(code1Rect.at<int>(r,c) != -1)
- 
 
 234 
//                code1Rect.at<int>(r,c) = grayToBinary(code1Rect.at<int>(r,c));
- 
 
 235 
//        }
- 
 
 236 
//    }
- 
 
 237 
 
- 
 
 238 
//cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
- 
 
 239 
//cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
- 
 
 240 
 
217 
    // matching
 241 
    // matching
218 
    std::vector<cv::Vec2f> q0Rect, q1Rect;
 242 
    std::vector<cv::Vec2f> q0Rect, q1Rect;
219 
    for(int row=0; row<frameRectRows; row++){
 243 
    for(int row=0; row<frameRectRows; row++){
220 
 
 244 
 
221 
        std::vector<cv::Vec4f> edges0, edges1;
 245 
        std::vector<cv::Vec4f> edges0, edges1;