test and refactor(testInterzoneSinusFourGrayscalePattern.cpp/testShif…

…tGrayCodePattern.cpp): add precies test and refactor src/cameras/. 1. create precise test images by blender. 2. add simulation test case for interzonSinusFourGrayscaePattern and shiftGraycodePattern. 3. dived src/cameras folder to mono/bino/trinocular folder.
Practice3DVision · Apr 15, 2024 · 1b62f85 · 1b62f85
1 parent f4848df
commit 1b62f85
Show file tree

Hide file tree

Showing 50 changed files with 527 additions and 68 deletions.
diff --git a/data/interzoneFourGrayscale/0.bmp b/data/interzoneFourGrayscale/0.bmp
diff --git a/data/interzoneFourGrayscale/1.bmp b/data/interzoneFourGrayscale/1.bmp
diff --git a/data/interzoneFourGrayscale/2.bmp b/data/interzoneFourGrayscale/2.bmp
diff --git a/data/interzoneFourGrayscale/3.bmp b/data/interzoneFourGrayscale/3.bmp
diff --git a/data/interzoneFourGrayscale/4.bmp b/data/interzoneFourGrayscale/4.bmp
diff --git a/data/shiftGraycode/0.bmp b/data/shiftGraycode/0.bmp
diff --git a/data/shiftGraycode/1.bmp b/data/shiftGraycode/1.bmp
diff --git a/data/shiftGraycode/2.bmp b/data/shiftGraycode/2.bmp
diff --git a/data/shiftGraycode/3.bmp b/data/shiftGraycode/3.bmp
diff --git a/data/shiftGraycode/4.bmp b/data/shiftGraycode/4.bmp
diff --git a/data/shiftGraycode/5.bmp b/data/shiftGraycode/5.bmp
diff --git a/data/shiftGraycode/6.bmp b/data/shiftGraycode/6.bmp
diff --git a/data/shiftGraycode/7.bmp b/data/shiftGraycode/7.bmp
diff --git a/data/shiftGraycode/8.bmp b/data/shiftGraycode/8.bmp
diff --git a/gui/qml/ui/global/Lang.qml b/gui/qml/ui/global/Lang.qml
@@ -45,6 +45,7 @@ QtObject {
     property string camera
     property string stripe_type
     property string sine_complementary_gray_code
+    property string interzone_sinus_four_grayscale
     property string encode
     property string save
     property string defocus_encoding
@@ -307,6 +308,7 @@ QtObject {
         stripe_type = "条纹类型";
         sine_complementary_gray_code = "正弦互补格雷码";
         sine_shift_gray_code = "正弦移位格雷码";
+        interzone_sinus_four_grayscale = "四灰度编码广义分区间相位展开";
         encode = "编码";
         save = "保存";
         defocus_encoding = "离焦编码";
@@ -788,6 +790,7 @@ QtObject {
         sequenceInverse = "Feature Point Sequence Inverse";
         horizonVerticalInv = "Horizon/Vertical Feature Point Sequence Inverse";
         useCurrentFeaturePoints = "Use Current Feature Points";
+        interzone_sinus_four_grayscale = "Interzone Sinus Four Grayscale";
     }
 
     property string __locale

diff --git a/gui/qml/ui/page/Page_Device.qml b/gui/qml/ui/page/Page_Device.qml
@@ -611,7 +611,7 @@ FluContentPage{
 
                                 FluComboBox {
                                     Layout.fillWidth: true
-                                    model: [Lang.sine_complementary_gray_code, Lang.multi_frequency_heterodyne, Lang.multi_view_stereo_geometry, Lang.sine_shift_gray_code]
+                                    model: [Lang.sine_complementary_gray_code, Lang.multi_frequency_heterodyne, Lang.multi_view_stereo_geometry, Lang.sine_shift_gray_code, Lang.interzone_sinus_four_grayscale]
                                     currentIndex: root.stripe_type
 
                                     onCurrentIndexChanged: {

diff --git a/gui/qml/ui/page/Page_Scan.qml b/gui/qml/ui/page/Page_Scan.qml
@@ -268,7 +268,7 @@ FluContentPage {
 
                             FluComboBox {
                                 Layout.fillWidth: true
-                                model: [Lang.sinus_comple_gray_code, Lang.multi_frequency_heterodyne, Lang.multi_view_stereo_geometry, Lang.sine_shift_gray_code]
+                                model: [Lang.sinus_comple_gray_code, Lang.multi_frequency_heterodyne, Lang.multi_view_stereo_geometry, Lang.sine_shift_gray_code, Lang.interzone_sinus_four_grayscale]
                                 currentIndex: root.cur_method
 
                                 onCurrentIndexChanged: {

diff --git a/gui/src/AppType.h b/gui/src/AppType.h
@@ -89,6 +89,7 @@ class AppType : public QObject
         MutiplyFrequency,
         MultiViewStereoGeometry,
         SinusShiftGrayCode,
+        InterzoneSinusFourGrayscale,
     };
 
     Q_ENUM(PageType)

diff --git a/gui/src/CameraEngine.cpp b/gui/src/CameraEngine.cpp
@@ -100,6 +100,16 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction,
         params.shiftTime_ = shiftTime;
 
         pattern = MonoSinusShiftGrayCodePattern::create(params);
+    } else if (stripeType == AppType::PatternMethod::InterzoneSinusFourGrayscale) {
+        MonoInterzoneSinusFourGrayscalePattern::Params params;
+
+        params.height_ = imgHeight;
+        params.width_ = imgWidth;
+        params.cycles_ = cycles;
+        params.horizontal_ = direction == AppType::Direction::Horizion;
+        params.shiftTime_ = shiftTime;
+
+        pattern = MonoInterzoneSinusFourGrayscalePattern::create(params);
     }
 
     std::vector<cv::Mat> imgs;
@@ -654,6 +664,22 @@ void CameraEngine::setPatternType(const int patternType) {
 
             pattern_ = BinoSinusShiftGrayCodePattern::create(params);
         }
+        else if(patternType_ == AppType::PatternMethod::InterzoneSinusFourGrayscale) {
+            BinoInterzoneSinusFourGrayscalePattern::Params params;
+
+            params.shiftTime_ = std::round(shiftTime);
+            params.cycles_ = std::round(cycles);
+            params.horizontal_ = !isVertical;
+            params.width_ = std::stoi(dlpWidth);
+            params.height_ = std::stoi(dlpHeight);
+            params.confidenceThreshold_ = confidenceThreshold;
+            params.maxCost_ = maxCost;
+            params.minDisparity_ = minDisp;
+            params.maxDisparity_ = maxDisp;
+            params.costMinDiff_ = costMinDiff;
+
+            pattern_ = BinoInterzoneSinusFourGrayscalePattern::create(params);
+        }
     }
     if (cameraType_ == AppType::CameraType::MonocularSLCamera) {
         if (patternType_ == AppType::PatternMethod::SinusCompleGrayCode) {
@@ -714,7 +740,38 @@ void CameraEngine::setPatternType(const int patternType) {
             cv::cv2eigen(PR4, params.PR4_);
 
             pattern_ = MonoSinusShiftGrayCodePattern::create(params);
-        } else if (patternType == AppType::PatternMethod::MutiplyFrequency) {
+        } 
+        else if (patternType == AppType::PatternMethod::MutiplyFrequency) {
+        }
+        else if(patternType_ == AppType::PatternMethod::InterzoneSinusFourGrayscale) {
+            MonoInterzoneSinusFourGrayscalePattern::Params params;
+
+            params.shiftTime_ = std::round(shiftTime);
+            params.cycles_ = std::round(cycles);
+            params.horizontal_ = !isVertical;
+            params.width_ = std::stoi(dlpWidth);
+            params.height_ = std::stoi(dlpHeight);
+            params.confidenceThreshold_ = confidenceThreshold;
+            params.minDepth_ = minDepth;
+            params.maxDepth_ = maxDepth;
+
+            cv::Mat PL1 = cv::Mat::eye(4, 4, CV_32FC1);
+            slCamera->getCaliInfo()->info_.M1_.copyTo(
+                PL1(cv::Rect(0, 0, 3, 3)));
+            cv::cv2eigen(PL1, params.PL1_);
+
+            cv::Mat PR4 = cv::Mat::eye(4, 4, CV_32FC1);
+            slCamera->getCaliInfo()->info_.Rlp_.copyTo(
+                PR4(cv::Rect(0, 0, 3, 3)));
+            slCamera->getCaliInfo()->info_.Tlp_.copyTo(
+                PR4(cv::Rect(3, 0, 1, 3)));
+            cv::Mat M4Normal = cv::Mat::eye(4, 4, CV_32FC1);
+            slCamera->getCaliInfo()->info_.M4_.copyTo(
+                M4Normal(cv::Rect(0, 0, 3, 3)));
+            PR4 = M4Normal * PR4;
+            cv::cv2eigen(PR4, params.PR4_);
+
+            pattern_ = MonoInterzoneSinusFourGrayscalePattern::create(params);
         }
     } else if (patternType == AppType::PatternMethod::MultiViewStereoGeometry) {
 #ifdef OPENCV_WITH_CUDA_MODULE

diff --git a/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.cpp b/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.cpp
@@ -53,7 +53,7 @@ class InterzoneSinusFourGrayscalePattern_Impl final
 
   private:
     // threshod four grayscale floor img
-    void threshod(const Mat &img, std::vector<float> &threshodVal,
+    void threshod(const Mat &img, const Mat &confidenceMap, std::vector<float> &threshodVal,
                   Mat &out) const;
     // k-means cluster
     float kMeans(const Mat &img, const Mat &confidenceMap,
@@ -197,7 +197,7 @@ float InterzoneSinusFourGrayscalePattern_Impl::kMeans(
 
             for (int j = 0; j < img.cols; ++j) {
                 // skip lower confidence pixels
-                if (ptrConfidenceMap[j] < params.confidenceThreshold) {
+                if (ptrConfidenceMap[j] < params.confidenceThreshold || isnan(ptrImg[j])) {
                     continue;
                 }
                 // find minimum distance
@@ -206,7 +206,7 @@ float InterzoneSinusFourGrayscalePattern_Impl::kMeans(
                 for (int k = 0; k < 4; ++k) {
                     float distance = abs(ptrImg[j] - threshod[k]);
 
-                    if (distance < minDistance) {
+                    if (distance <= minDistance) {
                         minDistance = distance;
                         minDistanceK = k;
                     }
@@ -221,7 +221,15 @@ float InterzoneSinusFourGrayscalePattern_Impl::kMeans(
     });
 
     for (int k = 0; k < 4; ++k) {
-        threshod[k] = sumGray[k].first / sumGray[k].second;
+        float curThreshod = sumGray[k].first / sumGray[k].second;
+        float diff = curThreshod - threshod[k];
+        //skip too big change
+        if(sumGray[k].second == 0 || abs(diff) > 0.05f) {
+            threshod[k] += curThreshod > threshod[k] ? 0.05f : -0.05f;
+            continue;
+        }
+
+        threshod[k] = curThreshod;
     }
 
     score /= (sumGray[0].second + sumGray[1].second + sumGray[2].second +
@@ -231,7 +239,7 @@ float InterzoneSinusFourGrayscalePattern_Impl::kMeans(
 }
 
 void InterzoneSinusFourGrayscalePattern_Impl::threshod(
-    const Mat &img, std::vector<float> &threshodVal, Mat &out) const {
+    const Mat &img, const Mat &confidenceMap, std::vector<float> &threshodVal, Mat &out) const {
     CV_Assert(!img.empty() && threshodVal.size() == 4);
 
     out = Mat::zeros(img.size(), CV_8UC1);
@@ -243,9 +251,14 @@ void InterzoneSinusFourGrayscalePattern_Impl::threshod(
     parallel_for_(Range(0, img.rows), [&](const Range &range) {
         for (int i = range.start; i < range.end; ++i) {
             auto ptrImg = img.ptr<float>(i);
+            auto ptrConfidence = confidenceMap.ptr<float>(i);
             auto ptrOut = out.ptr<uchar>(i);
 
             for (int j = 0; j < img.cols; ++j) {
+                if(ptrConfidence[j] < params.confidenceThreshold) {
+                    continue;
+                }
+
                 if (ptrImg[j] >= -FLT_MAX &&
                     ptrImg[j] < threshodVal[0] + half01) {
                     ptrOut[j] = 0;
@@ -327,9 +340,9 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeFloorMap(
 
         do {
             score = kMeans(normalizeGrayImgs[i], confidence, threshodVal);
-        } while (++count < 5 && score > 20.f);
+        } while (++count < 3 && score > 0.01f);
 
-        threshod(normalizeGrayImgs[i], threshodVal, threshodGrayImgs[i]);
+        threshod(normalizeGrayImgs[i], confidence, threshodVal, threshodGrayImgs[i]);
     }
     // compute floor map
     parallel_for_(Range(0, height), [&](const Range &range) {
@@ -341,8 +354,13 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeFloorMap(
             }
 
             auto ptrFloor = floor.ptr<uint16_t>(i);
+            auto ptrConfidence = confidence.ptr<float>(i);
 
             for (int j = 0; j < width; ++j) {
+                if(ptrConfidence[j] < params.confidenceThreshold) {
+                    continue;
+                }
+
                 int val = 0;
 
                 for (int k = 0; k < imgsPtrs.size(); ++k) {

diff --git a/src/calibration/concentricRingCalibrator.cpp b/src/calibration/concentricRingCalibrator.cpp
@@ -155,7 +155,8 @@ bool ConcentricRingCalibrator::findConcentricRingGrid(
         pointsOfCell.clear();
         isFind = cv::findCirclesGrid(
             threshodFindCircle, cv::Size(patternSize.width, patternSize.height),
-            pointsOfCell, cv::CALIB_CB_SYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING, detector);
+            pointsOfCell, cv::CALIB_CB_SYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING,
+            detector);
 
         if (!isFind) {
             return false;

diff --git a/src/cameras/binoocularCamer.cpp → src/cameras/binocular/binoocularCamer.cpp b/src/cameras/binoocularCamer.cpp → src/cameras/binocular/binoocularCamer.cpp
@@ -1,7 +1,7 @@
 #include "binoocularCamera.h"
 
 #include "binosSinusCompleGrayCodePattern.h"
-#include "tool.h"
+#include "../tool.h"
 
 namespace slmaster {
 namespace cameras {

diff --git a/src/cameras/binoocularCamera.h → src/cameras/binocular/binoocularCamera.h b/src/cameras/binoocularCamera.h → src/cameras/binocular/binoocularCamera.h
@@ -13,7 +13,7 @@
 #define __BINOCULAR_CAMERA_H_
 
 #include "../../device.h"
-#include "slCamera.h"
+#include "../slCamera.h"
 
 
 #include <unordered_map>

diff --git a/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.cpp b/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.cpp
@@ -0,0 +1,82 @@
+#include "binosInterzoneSinusFourGrayscalePattern.h"
+
+#include "../algorithm/algorithm.h"
+
+using namespace cv;
+
+namespace slmaster {
+namespace cameras {
+
+static BinoInterzoneSinusFourGrayscalePattern::Params params__;
+
+BinoInterzoneSinusFourGrayscalePattern::BinoInterzoneSinusFourGrayscalePattern() {}
+
+bool BinoInterzoneSinusFourGrayscalePattern::generate(
+    std::vector<cv::Mat> &imgs) const {
+    algorithm::InterzoneSinusFourGrayscalePattern::Params params;
+    params.width = params__.width_;
+    params.height = params__.height_;
+    params.nbrOfPeriods = params__.cycles_;
+    params.horizontal = params__.horizontal_;
+    params.maxCost = params__.maxCost_;
+    params.minDisparity = params__.minDisparity_;
+    params.maxDisparity = params__.maxDisparity_;
+    params.confidenceThreshold = params__.confidenceThreshold_;
+    params.shiftTime = params__.shiftTime_;
+
+    return algorithm::InterzoneSinusFourGrayscalePattern::create(params)
+        ->generate(imgs);
+}
+
+std::shared_ptr<Pattern> BinoInterzoneSinusFourGrayscalePattern::create(const Params& params) {
+    params__ = params;
+
+    return std::make_shared<BinoInterzoneSinusFourGrayscalePattern>();
+}
+
+bool BinoInterzoneSinusFourGrayscalePattern::decode(
+    const std::vector<std::vector<cv::Mat>> &patternImages,
+    cv::Mat &disparityMap, const bool isGpu) const {
+    CV_Assert(patternImages.size() >= 2);
+
+#ifdef OPENCV_WITH_CUDA_MODULE
+    if (isGpu) {
+        /*
+        algorithm::SinusCompleGrayCodePatternGPU::Params params;
+        params.shiftTime = params__.shiftTime_;
+        params.confidenceThreshold = params__.confidenceThreshold_;
+        params.costMinDiff = params__.costMinDiff_;
+        params.maxCost = params__.maxCost_;
+        params.height = params__.height_;
+        params.width = params__.width_;
+        params.nbrOfPeriods = params__.cycles_;
+        params.horizontal = params__.horizontal_;
+        params.minDisparity = params__.minDisparity_;
+        params.maxDisparity = params__.maxDisparity_;
+
+        auto pattern = algorithm::SinusCompleGrayCodePatternGPU::create(params);
+        return pattern->decode(patternImages, disparityMap,
+                               algorithm::SINUSOIDAL_COMPLEMENTARY_GRAY_CODE_GPU);
+        */
+        return false;
+    }
+#endif
+
+    algorithm::InterzoneSinusFourGrayscalePattern::Params params;
+    params.shiftTime = params__.shiftTime_;
+    params.width = params__.width_;
+    params.height = params__.height_;
+    params.nbrOfPeriods = params__.cycles_;
+    params.horizontal = params__.horizontal_;
+    params.maxCost = params__.maxCost_;
+    params.minDisparity = params__.minDisparity_;
+    params.maxDisparity = params__.maxDisparity_;
+    params.confidenceThreshold = params__.confidenceThreshold_;
+
+    auto pattern = algorithm::InterzoneSinusFourGrayscalePattern::create(params);
+    return pattern->decode(
+        patternImages, disparityMap, cv::noArray(), cv::noArray(),
+        algorithm::SINUSOIDAL_COMPLEMENTARY_GRAY_CODE);
+}
+} // namespace cameras
+} // namespace slmaster
diff --git a/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.h b/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.h
@@ -0,0 +1,50 @@
+/**
+ * @file binoSInterzoneSinusFourGrayscalePattern.h
+ * @author Evans Liu (1369215984@qq.com)
+ * @brief 
+ * @version 0.1
+ * @date 2024-04-15
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#ifndef __BINOS_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_
+#define __BINOS_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_
+
+#include "../../common.h"
+#include "../pattern.h"
+
+namespace slmaster {
+namespace cameras {
+class SLMASTER_API BinoInterzoneSinusFourGrayscalePattern : public Pattern {
+  public:
+    struct SLMASTER_API Params {
+        Params()
+            : width_(1920), height_(1080), shiftTime_(3), cycles_(16),
+              horizontal_(false), confidenceThreshold_(5.f), minDisparity_(0),
+              maxDisparity_(300), maxCost_(0.1f), costMinDiff_(0.0001f) {}
+        int width_;
+        int height_;
+        int shiftTime_;
+        int cycles_;
+        bool horizontal_;
+        float confidenceThreshold_;
+        int minDisparity_;
+        int maxDisparity_;
+        float maxCost_;
+        float costMinDiff_;
+    };
+
+    BinoInterzoneSinusFourGrayscalePattern();
+    virtual bool generate(IN std::vector<cv::Mat> &imgs) const override final;
+    virtual bool
+    decode(IN const std::vector<std::vector<cv::Mat>> &patternImages,
+           OUT cv::Mat &disparityMap, IN const bool isGpu) const override final;
+    static std::shared_ptr<Pattern> create(const Params& params);
+  private:
+};
+} // namespace cameras
+} // namespace slmaster
+
+#endif // __BINOS_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_
diff --git a/...meras/binosSinusCompleGrayCodePattern.cpp → ...cular/binosSinusCompleGrayCodePattern.cpp b/...meras/binosSinusCompleGrayCodePattern.cpp → ...cular/binosSinusCompleGrayCodePattern.cpp
@@ -1,6 +1,6 @@
 #include "binosSinusCompleGrayCodePattern.h"
 
-#include "../algorithm/algorithm.h"
+#include "../../algorithm/algorithm.h"
 
 using namespace cv;