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

 //
 // Two Frequency Phase Shifting using the Heterodyne Principle
 //
-// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)
+// This implementation follows "Reich, Ritter, Thesing, White light heterodyne
+// principle for 3D-measurement", SPIE (1997).
+//
 // Different from the paper, it uses only two different frequencies.
 //
-// The number of periods in the primary frequency can be chosen freely, but small changes can have a considerable impact on quality.
+// The number of periods in the primary frequency can be chosen freely, but
+// small changes can have a considerable impact on quality. The number of
-// The number of phase shifts can be chosen freely (min. 3), and higher values reduce the effects of image noise. They also allow us to filter bad points based on energy at non-primary frequencies.
+// phase shifts can be chosen freely (min. 3), and higher values reduce the
+// effects of image noise.They also allow us to filter bad points based on
+// energy at non-primary frequencies.
 //
 #include "AlgorithmPhaseShiftTwoFreq.h"
 #include <math.h>
 static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary
 static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary
 static float nPeriodsPrimary = 40; // primary period
-AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(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 to fulfill the heterodyne condition
 };
 static void getStereoRectifyier(const SMCalibrationParameters &calibration,
                                 const cv::Size& frameSize,
                                 StereoRectifyier& stereoRect);
 static void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
-                                    cv::Mat& amplitude, cv::Mat& phase, cv::Mat& energy);
+                                    cv::Mat& amplitude,
+                                    cv::Mat& phase,
+                                    cv::Mat& energy);
-static void collectPhases(const cv::Mat& phasePrimary, const cv::Mat& phaseSecondary,
+static void collectPhases(const cv::Mat& phasePrimary,
+                          const cv::Mat& phaseSecondary,
                           cv::Mat& phase);
 static void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
                            const cv::Mat& phase0, const cv::Mat& phase1,
                            std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1);
 static void triangulate(const StereoRectifyier& stereoRect,
                         const std::vector<cv::Vec2f>& q0,
                         const std::vector<cv::Vec2f>& q1,
                         std::vector<cv::Point3f>& Q);
+void AlgorithmPhaseShiftTwoFreq::
-void AlgorithmPhaseShiftTwoFreq::get3DPoints(const SMCalibrationParameters & calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3b>& color){
+    get3DPoints(const SMCalibrationParameters & calibration,
+                const std::vector<cv::Mat>& frames0,
+                const std::vector<cv::Mat>& frames1,
+                std::vector<cv::Point3f>& Q,
+                std::vector<cv::Vec3b>& color){
     assert(frames0.size() == N);
     assert(frames1.size() == N);
     StereoRectifyier stereoRect;
+    getStereoRectifyier(calibration,
-    getStereoRectifyier(calibration, cv::Size(frames0[0].cols, frames0[0].rows), stereoRect);
+                        cv::Size(frames0[0].cols, frames0[0].rows),
+                        stereoRect);
     // // Erode occlusion masks
     // cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
     cv::Mat up0, up1;
             std::vector<cv::Mat> frames0Rect(N);
             for(unsigned int i=0; i<N; i++){
                 cv::Mat temp;
                 cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
+                cv::remap(temp, frames0Rect[i],
-                cv::remap(temp, frames0Rect[i], stereoRect.map0X, stereoRect.map0Y, CV_INTER_LINEAR);
+                          stereoRect.map0X, stereoRect.map0Y,
+                          CV_INTER_LINEAR);
+            }
+            cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
+            cv::remap(color0, color0,
+                      stereoRect.map1X, stereoRect.map1Y,
+                      CV_INTER_LINEAR);
             // Occlusion masks
             cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
             occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
             // Decode camera0
-            std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
+            std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2,
+                                                frames0Rect.begin()+2+nStepsPrimary);
-            std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end());
+            std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary,
+                                                  frames0Rect.end());
             frames0Rect.clear();
             cv::Mat amplitude0Primary, amplitude0Secondary;
             cv::Mat up0Primary, up0Secondary;
             cv::Mat energy0Primary, energy0Secondary;
             determineAmplitudePhaseEnergy(frames0Primary,
-                                          amplitude0Primary, up0Primary, energy0Primary);
+                                          amplitude0Primary,
+                                          up0Primary,
+                                          energy0Primary);
             determineAmplitudePhaseEnergy(frames0Secondary,
-                                          amplitude0Secondary, up0Secondary, energy0Secondary);
+                                          amplitude0Secondary,
+                                          up0Secondary,
+                                          energy0Secondary);
             collectPhases(up0Primary, up0Secondary, up0);
             // Threshold on energy at primary frequency
             occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);
             #ifdef SM_DEBUG
                 cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
                 cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
                 cvtools::writeMat(up0, "up0.mat", "up0");
+                cvtools::writeMat(amplitude0Primary,
-                cvtools::writeMat(amplitude0Primary, "amplitude0Primary.mat", "amplitude0Primary");
+                                  "amplitude0Primary.mat", "amplitude0Primary");
+                cvtools::writeMat(amplitude0Secondary,
-                cvtools::writeMat(amplitude0Secondary, "amplitude0Secondary.mat", "amplitude0Secondary");
+                                  "amplitude0Secondary.mat", "amplitude0Secondary");
+                cvtools::writeMat(energy0Primary,
-                cvtools::writeMat(energy0Primary, "energy0Primary.mat", "energy0Primary");
+                                  "energy0Primary.mat", "energy0Primary");
+                cvtools::writeMat(energy0Secondary,
-                cvtools::writeMat(energy0Secondary, "energy0Secondary.mat", "energy0Secondary");
+                                  "energy0Secondary.mat", "energy0Secondary");
                 cvtools::writeMat(edges0, "edges0.mat", "edges0");
                 cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
+                cvtools::writeMat(color0, "color0.mat", "color0");
             #endif
+        }
         #pragma omp section
+        {
             std::vector<cv::Mat> frames1Rect(N);
             for(unsigned int i=0; i<N; i++){
                 cv::Mat temp;
                 cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+                cv::remap(temp, frames1Rect[i],
-                cv::remap(temp, frames1Rect[i], stereoRect.map1X, stereoRect.map1Y, CV_INTER_LINEAR);
+                          stereoRect.map1X, stereoRect.map1Y,
+                          CV_INTER_LINEAR);
+            }
+            cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
+            cv::remap(color1, color1,
+                      stereoRect.map1X, stereoRect.map1Y,
+                      CV_INTER_LINEAR);
             // Occlusion masks
             cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
             occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
             // Decode camera1
-            std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
+            std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2,
+                                                frames1Rect.begin()+2+nStepsPrimary);
-            std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
+            std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary,
+                                                  frames1Rect.end());
             frames1Rect.clear();
             cv::Mat amplitude1Primary, amplitude1Secondary;
             cv::Mat up1Primary, up1Secondary;
             cv::Mat energy1Primary, energy1Secondary;
             determineAmplitudePhaseEnergy(frames1Primary,
-                                          amplitude1Primary, up1Primary, energy1Primary);
+                                          amplitude1Primary,
+                                          up1Primary,
+                                          energy1Primary);
             determineAmplitudePhaseEnergy(frames1Secondary,
-                                          amplitude1Secondary, up1Secondary, energy1Secondary);
+                                          amplitude1Secondary,
+                                          up1Secondary,
+                                          energy1Secondary);
             collectPhases(up1Primary, up1Secondary, up1);
             // Threshold on energy at primary frequency
             occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
             #ifdef SM_DEBUG
                 cvtools::writeMat(up1Primary, "up1Primary.mat", "up1Primary");
                 cvtools::writeMat(up1Secondary, "up1Secondary.mat", "up1Secondary");
                 cvtools::writeMat(up1, "up1.mat", "up1");
+                cvtools::writeMat(amplitude1Primary,
-                cvtools::writeMat(amplitude1Primary, "amplitude1Primary.mat", "amplitude1Primary");
+                                  "amplitude1Primary.mat", "amplitude1Primary");
+                cvtools::writeMat(amplitude1Secondary,
-                cvtools::writeMat(amplitude1Secondary, "amplitude1Secondary.mat", "amplitude1Secondary");
+                                  "amplitude1Secondary.mat", "amplitude1Secondary");
+                cvtools::writeMat(energy1Primary,
-                cvtools::writeMat(energy1Primary, "energy1Primary.mat", "energy1Primary");
+                                  "energy1Primary.mat", "energy1Primary");
+                cvtools::writeMat(energy1Secondary,
-                cvtools::writeMat(energy1Secondary, "energy1Secondary.mat", "energy1Secondary");
+                                  "energy1Secondary.mat", "energy1Secondary");
                 cvtools::writeMat(edges1, "edges1.mat", "edges1");
                 cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
+                cvtools::writeMat(color1, "color1.mat", "color1");
             #endif
+        }
+    }
         Q.resize(0);
         color.resize(0);
         return;
+    }
-    {
+    else {
-        // color debayer and remap/rectify
-        cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
-        cv::remap(color1, color1, stereoRect.map1X, stereoRect.map1Y, CV_INTER_LINEAR);
-        #ifdef SM_DEBUG
-            cvtools::writeMat(color0, "color0.mat", "color0");
-            cvtools::writeMat(color1, "color1.mat", "color1");
-        #endif
         // Retrieve color information
         color.resize(nMatches);
         for(unsigned int i=0; i<nMatches; i++){
             cv::Vec3b c0 = color0.at<cv::Vec3b>(int(q0[i][1]), int(q0[i][0]));
     // Triangulate points
     triangulate(stereoRect, q0, q1, Q);
+}
-void getStereoRectifyier(const SMCalibrationParameters &calibration, const cv::Size& frameSize, StereoRectifyier& stereoRect){
+void getStereoRectifyier(const SMCalibrationParameters &calibration,
+                         const cv::Size& frameSize,
+                         StereoRectifyier& stereoRect){
     // cv::stereoRectify segfaults unless R is double precision
     cv::Mat R, T;
     cv::Mat(calibration.R1).convertTo(R, CV_64F);
     cv::Mat(calibration.T1).convertTo(T, CV_64F);
                                 frameSize, CV_32F,
                                 stereoRect.map1X, stereoRect.map1Y);
+}
 void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
-                                   cv::Mat& amplitude, cv::Mat& phase, cv::Mat& energy) {
+                                   cv::Mat& amplitude,
+                                   cv::Mat& phase,
+                                   cv::Mat& energy) {
     std::vector<cv::Mat> fourier = getDFTComponents(frames);
     cv::phase(fourier[2], -fourier[3], phase);
+    }
     frames.clear();
+}
-void collectPhases(const cv::Mat& phasePrimary, const cv::Mat& phaseSecondary,
+void collectPhases(const cv::Mat& phasePrimary,
+                   const cv::Mat& phaseSecondary,
                    cv::Mat& phase) {
     cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
     phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
     phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
     phase *= phasePrimary.cols/(2.0*CV_PI);

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp @ 257 – Rev 233 → 234