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

         int nSteps = Nm[m];
         cv::Mat Am(nSteps, 2, CV_32F);
-        for(unsigned int i=0; i<nSteps; i++){
+        for(int i=0; i<nSteps; i++){
             float phase = 2.0*CV_PI/std::max(nSteps, 3) * i;
             Am.at<float>(i, 0) = std::cos(phase);
             Am.at<float>(i, 1) = -std::sin(phase);
+        }
         cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
         color[i] = 0.5*c0 + 0.5*c1;
+    }
-    // Triangulate points
+    // Triangulate by means of disparity projection
-    cv::Mat QMatHomogenous, QMat;
+    Q.resize(q0.size());
-    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
+    cv::Matx44f QRectx = cv::Matx44f(QRect);
-    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
+    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
-    // Undo rectification
+    #pragma omp parallel for
-    cv::Mat R0Inv;
+    for(unsigned int i=0; i<q0.size(); i++){
-    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
+        float disparity = q0[i][0]-q1[i][0];
+        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
-    QMat = R0Inv*QMat;
+        float winv = float(1.0)/Qih[3];
-    cvtools::matToPoints3f(QMat, Q);
+        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
+    }
+}

Subversion Repositories seema-scanner