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

-#include "AlgorithmPhaseShift.h"
+#include "AlgorithmPhaseShiftTwoFreq.h"
 #include <math.h>
 #include "cvtools.h"
 #ifndef M_PI
     #define M_PI 3.14159265358979323846
 #endif
-static unsigned int nStepsPrimary = 24; // number of shifts/steps in primary
+static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary
-static unsigned int nStepsSecondary = 12; // number of shifts/steps in secondary
+static unsigned int nStepsSecondary = 6; // number of shifts/steps in secondary
-static float periodPrimary = 24; // primary period
+static float periodPrimary = 32; // primary period
 // Algorithm
 static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){
     cv::Mat phaseVector(length, 1, CV_8UC3);
+    }
     return phaseVector;
+}
-AlgorithmPhaseShift::AlgorithmPhaseShift(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
+AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
     // Set N
     N = 2+nStepsPrimary+nStepsSecondary;
     // Determine the secondary (wider) period
+    }
+}
-cv::Mat AlgorithmPhaseShift::getEncodingPattern(unsigned int depth){
+cv::Mat AlgorithmPhaseShiftTwoFreq::getEncodingPattern(unsigned int depth){
     return patterns[depth];
+}
 // Absolute phase from 3 frames
-cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){
+static cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){
     cv::Mat_<float> I1_(I1);
     cv::Mat_<float> I2_(I2);
     cv::Mat_<float> I3_(I3);
     return phase;
+}
 // Phase unwrapping by means of a phase cue
-cv::Mat unwrapWithCue(const cv::Mat up, const cv::Mat upCue, float nPhases){
+static cv::Mat unwrapWithCue(const cv::Mat up, const cv::Mat upCue, float nPhases){
     const float pi = M_PI;
     // Determine number of jumps
-    cv::Mat P = (upCue*nPhases-up)/(2*pi);
+    cv::Mat P = (upCue*nPhases-up)/(2.0*pi);
     // Round to integers
     P.convertTo(P, CV_8U);
     P.convertTo(P, CV_32F);
     return upUnwrapped;
+}
 // Absolute phase and magnitude from N frames
-std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> frames){
+static std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> frames){
     unsigned int N = frames.size();
 //    std::vector<cv::Mat> framesReverse = frames;
 //    std::reverse(framesReverse.begin(), framesReverse.end());
     return fIcomp;
+}
-void AlgorithmPhaseShift::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){
+void AlgorithmPhaseShiftTwoFreq::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){
     const float pi = M_PI;
     assert(frames0.size() == N);
     assert(frames1.size() == N);
     //cv::Mat up0Secondary = getPhase(frames0Secondary[0], frames0Secondary[1], frames0Secondary[2]);
     std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
     cv::Mat up0Secondary;
     cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
     cv::Mat up0Equivalent = up0Primary - up0Secondary;
-    up0Equivalent = cvtools::modulo(up0Equivalent, 2*pi);
+    up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
     cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/periodPrimary);
-    up0 *= screenCols/(2*pi);
+    up0 *= screenCols/(2.0*pi);
+    cv::Mat amplitude0;
+    cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
     // Decode camera1
     std::vector<cv::Mat> frames1Primary(frames1Gray.begin()+2, frames1Gray.begin()+2+nStepsPrimary);
     std::vector<cv::Mat> frames1Secondary(frames1Gray.begin()+2+nStepsPrimary, frames1Gray.end());
     std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
     //cv::Mat up1Secondary = getPhase(frames1Secondary[0], frames1Secondary[1], frames1Secondary[2]);
     std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
     cv::Mat up1Secondary;
     cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
     cv::Mat up1Equivalent = up1Primary - up1Secondary;
-    up1Equivalent = cvtools::modulo(up1Equivalent, 2*pi);
+    up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*pi);
     cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/periodPrimary);
-    up1 *= screenCols/(2*pi);
+    up1 *= screenCols/(2.0*pi);
+    cv::Mat amplitude1;
+    cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
+cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
+cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
+cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
+cvtools::writeMat(up0, "up0.mat", "up0");
+cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
     // Rectifying homographies (rotation+projections)
     cv::Size frameSize(frameCols, frameRows);
     cv::Mat R, T;
     // stereoRectify segfaults unless R is double precision
     // Phase remaps
     cv::Mat up0Rect, up1Rect;
     cv::remap(up0, up0Rect, map0X, map0Y, CV_INTER_LINEAR);
     cv::remap(up1, up1Rect, map1X, map1Y, CV_INTER_LINEAR);
+    // amplitude remaps
+    cv::Mat amplitude0Rect, amplitude1Rect;
+    cv::remap(amplitude0, amplitude0Rect, map0X, map0Y, CV_INTER_LINEAR);
+    cv::remap(amplitude1, amplitude1Rect, map1X, map1Y, CV_INTER_LINEAR);
 //cvtools::writeMat(up0Rect, "up0Rect.mat", "up0Rect");
 //cvtools::writeMat(up1Rect, "up1Rect.mat", "up1Rect");
     // color debayer and remap
     cv::Mat color0Rect, color1Rect;
     cv::subtract(frames0OnRect, frames0OffRect, occlusion0Rect);
     occlusion0Rect = (occlusion0Rect > 25) & (occlusion0Rect < 250);
     cv::subtract(frames1OnRect, frames1OffRect, occlusion1Rect);
     occlusion1Rect = (occlusion1Rect > 25) & (occlusion1Rect < 250);
+    // Threshold on energy at primary frequency
+    occlusion0Rect = occlusion0Rect & (amplitude0Rect > 5.0*nStepsPrimary);
+    occlusion1Rect = occlusion1Rect & (amplitude1Rect > 5.0*nStepsPrimary);
 //cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");
 //cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");
     // Erode occlusion masks
     cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
     cv::erode(occlusion0Rect, occlusion0Rect, strel);
     cv::erode(occlusion1Rect, occlusion1Rect, strel);
     // Threshold on gradient of phase
     cv::Mat edges0;
-    cv::Sobel(up0Rect, edges0, -1, 1, 0, 5);
+    cv::Sobel(up0Rect, edges0, -1, 1, 1, 5);
     occlusion0Rect = occlusion0Rect & (abs(edges0) < 150);
     cv::Mat edges1;
-    cv::Sobel(up1Rect, edges1, -1, 1, 0, 5);
+    cv::Sobel(up1Rect, edges1, -1, 1, 1, 5);
     occlusion1Rect = occlusion1Rect & (abs(edges1) < 150);
 //cvtools::writeMat(edges0, "edges0.mat", "edges0");
 //cvtools::writeMat(edges1, "edges1.mat", "edges1");

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp @ 257 – Rev 121 → 128