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

+    }
     // color remaps
     cv::Mat color0Rect, color1Rect;
     cv::remap(frames0[0], color0Rect, map0X, map0Y, CV_INTER_CUBIC);
-    cv::remap(frames1[0], color1Rect, map0X, map0Y, CV_INTER_CUBIC);
+    cv::remap(frames1[0], color1Rect, map1X, map1Y, CV_INTER_CUBIC);
     int frameRectRows = frames0Rect[0].rows;
     int frameRectCols = frames0Rect[0].cols;
     // occlusion maps
     for(int i=0; i<nMatches; i++){
         cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
         cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
-        color[i] = 0.5*(c0 + c1);
+        color[i] = 0.5*c0 + 0.5*c1;
+    }
     // triangulate points
     cv::Mat QMatHomogenous, QMat;
+//    cv::Mat C0 = P0.clone();
+//    cv::Mat C1 = P1.clone();
+//    C0.colRange(0, 3) = C0.colRange(0, 3)*R0;
+//    C1.colRange(0, 3) = C1.colRange(0, 3)*R1.t();
     cv::triangulatePoints(P0, P1, q0Rect, q1Rect, QMatHomogenous);
     cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
+    // undo rectification
+    cv::Mat R0Inv;
+    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
+    QMat = R0Inv*QMat;
     cvtools::matToPoints3f(QMat, Q);
+}

Subversion Repositories seema-scanner