WebSVN – seema-scanner – Diff – /src/algorithm/AlgorithmGrayCodeHorzVert.cpp

 typedef struct intersection{
     unsigned int row;
     unsigned int col;
+    // 64 bit integer (long long)
     unsigned long id;
     intersection() : row(0), col(0), id(0){}
     intersection(unsigned int _row, unsigned int _col, unsigned long long _id) :
         row(_row), col(_col), id(_id){}
 } intersection;
     int labelVert;
     int labelHorzRight;
     int labelVertBelow;
     // collect intersections
-    for(int row=0; row<nRows+1; row++){
+    for(int row=0; row<nRows-1; row++){
-        for(int col=0; col<nCols+1; col++){
+        for(int col=0; col<nCols-1; col++){
             labelHorz = codeHorz.at<int>(row, col);
             labelHorzRight = codeHorz.at<int>(row, col+1);
             labelVert = codeVert.at<int>(row, col);
                countBits(labelHorz^labelHorzRight) == 1 &&
                labelVert != -1 && labelVertBelow != -1 &&
                countBits(labelVert^labelVertBelow) == 1){
                 // shift together labels to form unique intersection id
-                unsigned long id = (labelHorz << NbitsHorz+2*NbitsVert) + (labelHorzRight << 2*NbitsVert) +
+                unsigned long id = ((ulong)labelHorz << (NbitsHorz+2*NbitsVert)) + ((ulong)labelHorzRight << (2*NbitsVert)) +
-                                   (labelVert << NbitsVert) + labelVertBelow;
+                                   ((ulong)labelVert << NbitsVert) + (ulong)labelVertBelow;
                 // store intersection
                 intersections.push_back(intersection(row, col, id));
+            }
+        }
     // subpixel refinement finds the intersection of linear interpolants in the positive/negative pattern
     for(int i=0; i<nMatches; i++){
         // shift the labels back out from id
-        int labelHorz = (matches[i].id >> NbitsHorz+2*NbitsVert) & (1 << (NbitsHorz+1) - 1);
+        int labelHorz = (matches[i].id >> (NbitsHorz+2*NbitsVert)) & ((1 << NbitsHorz) - 1);
-        int labelHorzRight = (matches[i].id >> 2*NbitsVert) & (1 << (NbitsHorz+1) - 1);
+        int labelHorzRight = (matches[i].id >> (2*NbitsVert)) & ((1 << NbitsHorz) - 1);
-        int labelVert = (matches[i].id >> NbitsVert) & (1 << (NbitsVert+1) - 1);
+        int labelVert = (matches[i].id >> NbitsVert) & ((1 << NbitsVert) - 1);
-        int labelVertBelow = matches[i].id  & (1 << (NbitsVert+1) - 1);
+        int labelVertBelow = matches[i].id  & ((1 << NbitsVert) - 1);
+//        std::cout << "id: " << matches[i].id << std::endl;
+//        std::cout << "labelHorz: " << labelHorz << std::endl;
+//        std::cout << "labelHorzRight: " << labelHorzRight << std::endl;
+//        std::cout << "labelVert: " << labelVert << std::endl;
+//        std::cout << "labelVertBelow: " << labelVertBelow << std::endl;
         // determine the levels at which the edges exists
         int levelHorz = NbitsHorz - leastSignificantBitSet(labelHorz^labelHorzRight);
         int levelVert = NbitsVert - leastSignificantBitSet(labelVert^labelVertBelow);
         // interpolate horizontal coordinate
         float row = matches[i].row;
         float col = matches[i].col;
         float colRight = col+1;
-        float posHorz = frames0[2*levelHorz+2].at<char>(row, col);
+        float posHorz = frames[2*levelHorz+2].at<char>(row, col);
-        float negHorz = frames0[2*levelHorz+3].at<char>(row, col);
+        float negHorz = frames[2*levelHorz+3].at<char>(row, col);
-        float posHorzRight = frames0[2*levelHorz+2].at<char>(row, colRight);
+        float posHorzRight = frames[2*levelHorz+2].at<char>(row, colRight);
-        float negHorzRight = frames0[2*levelHorz+3].at<char>(row, colRight);
+        float negHorzRight = frames[2*levelHorz+3].at<char>(row, colRight);
         float x = col + (posHorz - negHorz)/(negHorzRight - negHorz - posHorzRight + posHorz);
         // interpolate vertical coordinate
         float rowBelow = row+1;
-        float posVert = frames[2*NbitsHorz+2*levelVert+2].at<char>(rowBelow, col);
+        float posVert = frames[2*NbitsHorz+2*levelVert+2].at<char>(row, col);
-        float negVert = frames[2*NbitsHorz+2*levelVert+3].at<char>(rowBelow, col);
+        float negVert = frames[2*NbitsHorz+2*levelVert+3].at<char>(row, col);
         float posVertBelow = frames[2*NbitsHorz+2*levelVert+2].at<char>(rowBelow, col);
         float negVertBelow = frames[2*NbitsHorz+2*levelVert+3].at<char>(rowBelow, col);
         float y = row + (posVert - negVert)/(negVertBelow - negVert - posVertBelow + posVert);
         // write into return vector
-        q[i] = cv::Vec2f(x, y);
+        q[i] = cv::Point2f(x, y);
+    }
+}
 static cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
+    }
     // vertical codes into gray code
     for(int i=0; i<NbitsVert; i++){
         cv::Mat bit0;
-        cv::subtract(frames0Gray[i*2+NbitsHorz+2], frames0Gray[i*2+NbitsHorz+3], bit0);
+        cv::subtract(frames0Gray[i*2+2*NbitsHorz+2], frames0Gray[i*2+2*NbitsHorz+3], bit0);
         bit0 = bit0 > 0;
         bit0.convertTo(bit0, CV_32S, 1.0/255.0);
         cv::add(code0Vert, bit0*twopowi(NbitsVert-i-1), code0Vert, cv::Mat(), CV_32S);
         cv::Mat bit1;
-        cv::subtract(frames1Gray[i*2+NbitsHorz+2], frames1Gray[i*2+NbitsHorz+3], bit1);
+        cv::subtract(frames1Gray[i*2+2*NbitsHorz+2], frames1Gray[i*2+2*NbitsHorz+3], bit1);
         bit1 = bit1 > 0;
         bit1.convertTo(bit1, CV_32S, 1.0/255.0);
         cv::add(code1Vert, bit1*twopowi(NbitsVert-i-1), code1Vert, cv::Mat(), CV_32S);
+    }
-//cvtools::writeMat(code0Horz, "code0Horz.mat", "code0Horz");
-//cvtools::writeMat(code1Horz, "code1Horz.mat", "code1Horz");
-//cvtools::writeMat(code0Vert, "code0Vert.mat", "code0Vert");
-//cvtools::writeMat(code1Vert, "code1Vert.mat", "code1Vert");
     // set occluded pixels to -1
     for(int r=0; r<frameRows; r++){
         for(int c=0; c<frameCols; c++){
             if(occlusion0.at<char>(r,c) == 0){
-                code0Horz.at<float>(r,c) = -1;
+                code0Horz.at<int>(r,c) = -1;
-                code0Vert.at<float>(r,c) = -1;
+                code0Vert.at<int>(r,c) = -1;
+            }
             if(occlusion1.at<char>(r,c) == 0){
-                code1Horz.at<float>(r,c) = -1;
+                code1Horz.at<int>(r,c) = -1;
-                code1Vert.at<float>(r,c) = -1;
+                code1Vert.at<int>(r,c) = -1;
+            }
+        }
+    }
+//    cvtools::writeMat(code0Horz, "code0Horz.mat", "code0Horz");
+//    cvtools::writeMat(code1Horz, "code1Horz.mat", "code1Horz");
-    // matching
+//    cvtools::writeMat(code0Vert, "code0Vert.mat", "code0Vert");
-    std::vector<intersection> intersections0, intersections1;
+//    cvtools::writeMat(code1Vert, "code1Vert.mat", "code1Vert");
+    // get intersections
-    // intersection data structure containing [floor(row), floor(column), id]
+    std::vector<intersection> intersections0, intersections1;
     getIntersectionLabels(code0Horz, code0Vert, NbitsHorz, NbitsVert, intersections0);
     getIntersectionLabels(code1Horz, code1Vert, NbitsHorz, NbitsVert, intersections1);
     // match intersections
     std::vector<intersection> matches0, matches1;
+    }
     std::vector<cv::Point2f> q0(nMatches), q1(nMatches);
 //    for(int i=0; i<nMatches; i++){
-//        q0[i] = cv::Vec2f(matches0[i].col, matches0[i].row);
+//        q0[i] = cv::Point2f(matches0[i].col, matches0[i].row);
-//        q1[i] = cv::Vec2f(matches1[i].col, matches1[i].row);
+//        q1[i] = cv::Point2f(matches1[i].col, matches1[i].row);
 //    }
     // subpixel refinement
-    getSubpixelCoordinates(matches0, frames0, NbitsHorz, NbitsVert,q0);
+    getSubpixelCoordinates(matches0, frames0Gray, NbitsHorz, NbitsVert, q0);
-    getSubpixelCoordinates(matches1, frames1, NbitsHorz, NbitsVert,q1);
+    getSubpixelCoordinates(matches1, frames1Gray, NbitsHorz, NbitsVert, q1);
     // retrieve color information (at subpixel coordinates)
     color.resize(nMatches);
     for(int i=0; i<nMatches; i++){
-//        cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);
+        cv::Vec3b c0 = color0.at<cv::Vec3b>(std::floor(q0[i].y), std::floor(q0[i].x));
-//        cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
+        cv::Vec3b c1 = color1.at<cv::Vec3b>(std::floor(q1[i].y), std::floor(q1[i].x));
-        cv::Vec3b c0 = getColorSubpix(color0, q0[i]);
+//        cv::Vec3b c0 = getColorSubpix(color0, q0[i]);
-        cv::Vec3b c1 = getColorSubpix(color1, q1[i]);
+//        cv::Vec3b c1 = getColorSubpix(color1, q1[i]);
         color[i] = 0.5*c0 + 0.5*c1;
+    }
     // triangulate points
     cv::Mat P1(3, 4, CV_32F), temp(3,4,CV_32F);
     cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
     cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
     P1 = cv::Mat(calibration.K1) * temp;
-    //cv::correctMatches(calibration.F, q0, q1, q0, q1);
+    cv::correctMatches(calibration.F, q0, q1, q0, q1);
     cv::Mat QMatHomogenous, QMat;
     cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
     cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmGrayCodeHorzVert.cpp – Rev 108 → 109