Subversion Repositories seema-scanner

Rev

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

Rev 163 Rev 167
Line 39... Line 39...
39 
static bool getBit(int decimal, int N){
 39 
static bool getBit(int decimal, int N){
40 
 
 40 
 
41 
    return decimal & 1 << (N-1);
 41 
    return decimal & 1 << (N-1);
42 
}
 42 
}
43 
 
 43 
 
44 
/*
 44 
///*
45 
 * Return the number of bits set in an integer
 45 
// * Return the number of bits set in an integer
46 
 */
 46 
// */
47 
static int countBits(int n) {
 47 
//static int countBits(int n) {
48 
  unsigned int c; // c accumulates the total bits set in v
 48 
//  unsigned int c; // c accumulates the total bits set in v
49 
  for (c = 0; n>0; c++)
 49 
//  for (c = 0; n>0; c++)
50 
    n &= n - 1; // clear the least significant bit set
 50 
//    n &= n - 1; // clear the least significant bit set
51 
  return c;
 51 
//  return c;
52 
}
 52 
//}
53 
 
 53 
 
54 
/*
 54 
/*
55 
 * Return the position of the least significant bit that is set
 55 
 * Return the position of the least significant bit that is set
56 
 */
 56 
 */
57 
static int leastSignificantBitSet(int x){
 57 
static int leastSignificantBitSet(int x){
Line 166... Line 166...
166 
 
 166 
 
167 
cv::Mat AlgorithmGrayCodeMax::getEncodingPattern(unsigned int depth){
 167 
cv::Mat AlgorithmGrayCodeMax::getEncodingPattern(unsigned int depth){
168 
    return patterns[depth];
 168 
    return patterns[depth];
169 
}
 169 
}
170 
 
 170 
 
171 
static cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
 171 
//static cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
172 
    assert(!img.empty());
 172 
//    assert(!img.empty());
173 
    assert(img.channels() == 3);
 173 
//    assert(img.channels() == 3);
174 
 
 174 
 
175 
    int x = (int)pt.x;
 175 
//    int x = (int)pt.x;
176 
    int y = (int)pt.y;
 176 
//    int y = (int)pt.y;
177 
 
 177 
 
178 
    int x0 = cv::borderInterpolate(x,   img.cols, cv::BORDER_REFLECT_101);
 178 
//    int x0 = cv::borderInterpolate(x,   img.cols, cv::BORDER_REFLECT_101);
179 
    int x1 = cv::borderInterpolate(x+1, img.cols, cv::BORDER_REFLECT_101);
 179 
//    int x1 = cv::borderInterpolate(x+1, img.cols, cv::BORDER_REFLECT_101);
180 
    int y0 = cv::borderInterpolate(y,   img.rows, cv::BORDER_REFLECT_101);
 180 
//    int y0 = cv::borderInterpolate(y,   img.rows, cv::BORDER_REFLECT_101);
181 
    int y1 = cv::borderInterpolate(y+1, img.rows, cv::BORDER_REFLECT_101);
 181 
//    int y1 = cv::borderInterpolate(y+1, img.rows, cv::BORDER_REFLECT_101);
182 
 
 182 
 
183 
    float a = pt.x - (float)x;
 183 
//    float a = pt.x - (float)x;
184 
    float c = pt.y - (float)y;
 184 
//    float c = pt.y - (float)y;
185 
 
 185 
 
186 
    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)
 186 
//    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)
187 
                           + (img.at<cv::Vec3b>(y1, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[0] * a) * c);
 187 
//                           + (img.at<cv::Vec3b>(y1, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[0] * a) * c);
188 
    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)
 188 
//    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)
189 
                           + (img.at<cv::Vec3b>(y1, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[1] * a) * c);
 189 
//                           + (img.at<cv::Vec3b>(y1, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[1] * a) * c);
190 
    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)
 190 
//    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)
191 
                           + (img.at<cv::Vec3b>(y1, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[2] * a) * c);
 191 
//                           + (img.at<cv::Vec3b>(y1, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[2] * a) * c);
192 
 
 192 
 
193 
    return cv::Vec3b(b, g, r);
 193 
//    return cv::Vec3b(b, g, r);
194 
}
 194 
//}
195 
 
 195 
 
196 
void AlgorithmGrayCodeMax::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){
 196 
void AlgorithmGrayCodeMax::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){
197 
 
 197 
 
198 
    assert(frames0.size() == N);
 198 
    assert(frames0.size() == N);
199 
    assert(frames1.size() == N);
 199 
    assert(frames1.size() == N);
Line 330... Line 330...
330 
    // decode patterns
 330 
    // decode patterns
331 
    cv::Mat code0Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
 331 
    cv::Mat code0Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
332 
    cv::Mat code1Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
 332 
    cv::Mat code1Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
333 
 
 333 
 
334 
    // into gray code
 334 
    // into gray code
335 
    for(int i=0; i<Nbits; i++){
 335 
    for(unsigned int i=0; i<Nbits; i++){
336 
        cv::Mat temp, bit0, bit1;
 336 
        cv::Mat temp, bit0, bit1;
337 
 
 337 
 
338 
        cv::compare(frames0Rect[i*2+2], frames0Rect[i*2+3], temp, cv::CMP_GT);
 338 
        cv::compare(frames0Rect[i*2+2], frames0Rect[i*2+3], temp, cv::CMP_GT);
339 
        temp.convertTo(bit0, CV_32S, 1.0/255.0);
 339 
        temp.convertTo(bit0, CV_32S, 1.0/255.0);
340 
        cv::add(code0Rect, bit0*ggmax::twopowi(Nbits-i-1), code0Rect, cv::noArray(), CV_32S);
 340 
        cv::add(code0Rect, bit0*ggmax::twopowi(Nbits-i-1), code0Rect, cv::noArray(), CV_32S);
Line 399... Line 399...
399 
        ggmax::getEdgeLabels(code0Rect.row(row), Nbits, edges0);
 399 
        ggmax::getEdgeLabels(code0Rect.row(row), Nbits, edges0);
400 
        ggmax::getEdgeLabels(code1Rect.row(row), Nbits, edges1);
 400 
        ggmax::getEdgeLabels(code1Rect.row(row), Nbits, edges1);
401 
 
 401 
 
402 
        // match edges
 402 
        // match edges
403 
        std::vector<cv::Vec4i> matchedEdges0, matchedEdges1;
 403 
        std::vector<cv::Vec4i> matchedEdges0, matchedEdges1;
404 
        int i=0, j=0;
 404 
        unsigned int i=0, j=0;
405 
        while(i<edges0.size() && j<edges1.size()){
 405 
        while(i<edges0.size() && j<edges1.size()){
406 
 
 406 
 
407 
            if(edges0[i][3] == edges1[j][3]){
 407 
            if(edges0[i][3] == edges1[j][3]){
408 
                matchedEdges0.push_back(edges0[i]);
 408 
                matchedEdges0.push_back(edges0[i]);
409 
                matchedEdges1.push_back(edges1[j]);
 409 
                matchedEdges1.push_back(edges1[j]);
Line 416... Line 416...
416 
            }
 416 
            }
417 
        }
 417 
        }
418 
 
 418 
 
419 
        // crude subpixel refinement
 419 
        // crude subpixel refinement
420 
        // finds the intersection of linear interpolants in the positive/negative pattern
 420 
        // finds the intersection of linear interpolants in the positive/negative pattern
421 
        for(int i=0; i<matchedEdges0.size(); i++){
 421 
        for(unsigned int i=0; i<matchedEdges0.size(); i++){
422 
 
 422 
 
423 
            int level = Nbits - ggmax::leastSignificantBitSet(matchedEdges0[i][1]^matchedEdges0[i][2]);
 423 
            int level = Nbits - ggmax::leastSignificantBitSet(matchedEdges0[i][1]^matchedEdges0[i][2]);
424 
 
 424 
 
425 
            // refine for camera 0
 425 
            // refine for camera 0
426 
            float c0 = matchedEdges0[i][0];
 426 
            float c0 = matchedEdges0[i][0];