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

 typedef struct intersection{
     unsigned int row;
     unsigned int col;
-    unsigned int labelLeft;
-    unsigned int labelRight;
-    unsigned int labelAbove;
-    unsigned int labelBelow;
     unsigned long id;
-    intersection() : row(0), col(0), labelLeft(0), labelRight(0), labelAbove(0), labelBelow(0), id(0){}
+    intersection() : row(0), col(0), id(0){}
-    intersection(unsigned int _row, unsigned int _col, unsigned int _labelLeft, unsigned int _labelRight, unsigned int _labelAbove, unsigned int _labelBelow, unsigned long long _id) :
+    intersection(unsigned int _row, unsigned int _col, unsigned long long _id) :
-        row(_row), col(_col), labelLeft(_labelLeft), labelRight(_labelRight), labelAbove(_labelAbove), labelBelow(_labelBelow), id(_id){}
+        row(_row), col(_col), id(_id){}
 } intersection;
 static bool sortingLarger(intersection i,intersection j){ return (i.id<j.id);}
 static bool sortingEqual(intersection i,intersection j){ return (i.id==j.id);}
-static void getIntersectionLabels(const cv::Mat& codeHorz, const cv::Mat& codeVert, int NbitsHorz, int NbitsVert, std::vector<intersection>& intersections){
+static void getIntersectionLabels(const cv::Mat& codeHorz, const cv::Mat& codeVert, const int NbitsHorz, const int NbitsVert, std::vector<intersection>& intersections){
     int nRows = codeHorz.rows;
     int nCols = codeHorz.cols;
     int labelHorz;
             if(labelHorz != -1 && labelHorzRight != -1 &&
                countBits(labelHorz^labelHorzRight) == 1 &&
                labelVert != -1 && labelVertBelow != -1 &&
                countBits(labelVert^labelVertBelow) == 1){
-                // OVERFLOW??
+                // shift together labels to form unique intersection id
-                unsigned long id = ((ulong)labelHorz << NbitsHorz+2*NbitsVert) + ((ulong)labelHorzRight << 2*NbitsVert) +
+                unsigned long id = (labelHorz << NbitsHorz+2*NbitsVert) + (labelHorzRight << 2*NbitsVert) +
-                                          ((ulong)labelVert << NbitsVert) + (ulong)labelVertBelow;
+                                   (labelVert << NbitsVert) + labelVertBelow;
-                // store left label column
+                // store intersection
-                intersections.push_back(intersection(row, col, labelHorz, labelHorzRight, labelVert, labelVertBelow, id));
+                intersections.push_back(intersection(row, col, id));
+            }
+        }
+    }
     // sort
     std::sort(intersections.begin(), intersections.end(), sortingLarger);
     std::vector<intersection>::iterator it;
     it = std::unique(intersections.begin(), intersections.end(), sortingEqual);
     intersections.resize(std::distance(intersections.begin(),it));
+}
+static void getSubpixelCoordinates(const std::vector<intersection>& matches, const std::vector<cv::Mat>& frames, const int NbitsHorz, const int NbitsVert, std::vector<cv::Point2f> &q){
+    int nMatches = matches.size();
+    q.resize(nMatches);
+    // 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 labelHorzRight = (matches[i].id >> 2*NbitsVert) & (1 << (NbitsHorz+1) - 1);
+        int labelVert = (matches[i].id >> NbitsVert) & (1 << (NbitsVert+1) - 1);
+        int labelVertBelow = matches[i].id  & (1 << (NbitsVert+1) - 1);
+        // 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 negHorz = frames0[2*levelHorz+3].at<char>(row, col);
+        float posHorzRight = frames0[2*levelHorz+2].at<char>(row, colRight);
+        float negHorzRight = frames0[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 negVert = frames[2*NbitsHorz+2*levelVert+3].at<char>(rowBelow, 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);
+    }
+}
 static cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
     assert(!img.empty());
     assert(img.channels() == 3);
     int x = (int)pt.x;
         color.resize(0);
         return;
+    }
-    std::vector<cv::Vec2f> q0(nMatches), q1(nMatches);
+    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);
 //        q1[i] = cv::Vec2f(matches1[i].col, matches1[i].row);
 //    }
-    // horizontal subpixel refinement finds the intersection of linear interpolants in the positive/negative pattern
-    // TODO: subpixel refinement in both horizontal and vertical
-    for(int i=0; i<nMatches; i++){
-        int level = NbitsHorz - leastSignificantBitSet(matches0[i].labelLeft^matches0[i].labelRight);
-        // refine for camera 0
+    // subpixel refinement
-        float row = matches0[i].row;
-        float c0 = matches0[i].col;
-        float c1 = c0+1;
-        float pos0 = frames0[2*level+2].at<char>(row, c0);
-        float pos1 = frames0[2*level+2].at<char>(row, c1);
-        float neg0 = frames0[2*level+3].at<char>(row, c0);
+    getSubpixelCoordinates(matches0, frames0, NbitsHorz, NbitsVert,q0);
-        float neg1 = frames0[2*level+3].at<char>(row, c1);
+    getSubpixelCoordinates(matches1, frames1, NbitsHorz, NbitsVert,q1);
-        float col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
-        q0[i] = cv::Vec2f(col, row);
-        // refine for camera 1
-        row = matches1[i].row;
-        c0 = matches1[i].col;
-        c1 = c0+1;
-        pos0 = frames1[2*level+2].at<char>(row, c0);
-        pos1 = frames1[2*level+2].at<char>(row, c1);
-        neg0 = frames1[2*level+3].at<char>(row, c0);
-        neg1 = frames1[2*level+3].at<char>(row, c1);
-        col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
-        q1[i] = cv::Vec2f(col, row);
-    }
-    // retrieve color information (at integer coordinates)
+    // 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 c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
     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 107 → 108