diff --git a/data/threeFrequencyHeterodyne/0.bmp b/data/threeFrequencyHeterodyne/0.bmp new file mode 100644 index 0000000..f9e421e Binary files /dev/null and b/data/threeFrequencyHeterodyne/0.bmp differ diff --git a/data/threeFrequencyHeterodyne/1.bmp b/data/threeFrequencyHeterodyne/1.bmp new file mode 100644 index 0000000..7773bf6 Binary files /dev/null and b/data/threeFrequencyHeterodyne/1.bmp differ diff --git a/data/threeFrequencyHeterodyne/10.bmp b/data/threeFrequencyHeterodyne/10.bmp new file mode 100644 index 0000000..aaed475 Binary files /dev/null and b/data/threeFrequencyHeterodyne/10.bmp differ diff --git a/data/threeFrequencyHeterodyne/11.bmp b/data/threeFrequencyHeterodyne/11.bmp new file mode 100644 index 0000000..ed85d60 Binary files /dev/null and b/data/threeFrequencyHeterodyne/11.bmp differ diff --git a/data/threeFrequencyHeterodyne/2.bmp b/data/threeFrequencyHeterodyne/2.bmp new file mode 100644 index 0000000..6aaffce Binary files /dev/null and b/data/threeFrequencyHeterodyne/2.bmp differ diff --git a/data/threeFrequencyHeterodyne/3.bmp b/data/threeFrequencyHeterodyne/3.bmp new file mode 100644 index 0000000..3960e2f Binary files /dev/null and b/data/threeFrequencyHeterodyne/3.bmp differ diff --git a/data/threeFrequencyHeterodyne/4.bmp b/data/threeFrequencyHeterodyne/4.bmp new file mode 100644 index 0000000..11d71af Binary files /dev/null and b/data/threeFrequencyHeterodyne/4.bmp differ diff --git a/data/threeFrequencyHeterodyne/5.bmp b/data/threeFrequencyHeterodyne/5.bmp new file mode 100644 index 0000000..696c53b Binary files /dev/null and b/data/threeFrequencyHeterodyne/5.bmp differ diff --git a/data/threeFrequencyHeterodyne/6.bmp b/data/threeFrequencyHeterodyne/6.bmp new file mode 100644 index 0000000..98bde26 Binary files /dev/null and b/data/threeFrequencyHeterodyne/6.bmp differ diff --git a/data/threeFrequencyHeterodyne/7.bmp b/data/threeFrequencyHeterodyne/7.bmp new file mode 100644 index 0000000..6bf03c6 Binary files /dev/null and b/data/threeFrequencyHeterodyne/7.bmp differ diff --git a/data/threeFrequencyHeterodyne/8.bmp b/data/threeFrequencyHeterodyne/8.bmp new file mode 100644 index 0000000..bf4110b Binary files /dev/null and b/data/threeFrequencyHeterodyne/8.bmp differ diff --git a/data/threeFrequencyHeterodyne/9.bmp b/data/threeFrequencyHeterodyne/9.bmp new file mode 100644 index 0000000..e37111e Binary files /dev/null and b/data/threeFrequencyHeterodyne/9.bmp differ diff --git a/gui/qml/ui/global/Lang.qml b/gui/qml/ui/global/Lang.qml index a378533..a982d95 100644 --- a/gui/qml/ui/global/Lang.qml +++ b/gui/qml/ui/global/Lang.qml @@ -144,7 +144,7 @@ QtObject { property string enable_gpu property string texture property string sinus_comple_gray_code - property string multi_frequency_heterodyne + property string three_frequency_heterodyne property string reserved property string please_select property string select_finished @@ -407,7 +407,7 @@ QtObject { enable_gpu = "GPU加速"; texture = "纹理"; sinus_comple_gray_code = "正弦互补格雷码"; - multi_frequency_heterodyne = "多频外差"; + three_frequency_heterodyne = "三频外差"; reserved = "保留"; please_select = "请通过右键点击屏幕以绘制包围盒！"; select_finished = "包围盒绘制完成！"; @@ -672,7 +672,7 @@ QtObject { enable_gpu = "GPU Accelerate"; texture = "Texture"; sinus_comple_gray_code = "Sinusoidal Complementary Gray Code"; - multi_frequency_heterodyne = "Multi Frequency Heterodyne"; + three_frequency_heterodyne = "Three Frequency Heterodyne"; reserved = "Reserved"; please_select = "Please select end point through right button click to finish draw bounding box!"; select_finished = "Select bounding box finish!"; diff --git a/gui/qml/ui/page/Page_Device.qml b/gui/qml/ui/page/Page_Device.qml index 81ae0c5..e902a1e 100644 --- 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, Lang.interzone_sinus_four_grayscale] + model: [Lang.sine_complementary_gray_code, Lang.three_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 index 8d25a25..2027e7d 100644 --- 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, Lang.interzone_sinus_four_grayscale] + model: [Lang.sinus_comple_gray_code, Lang.three_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 index 0fabd1e..3737e1e 100644 --- a/gui/src/AppType.h +++ b/gui/src/AppType.h @@ -86,7 +86,7 @@ class AppType : public QObject enum PatternMethod { SinusCompleGrayCode = 0, - MutiplyFrequency, + ThreeFrequencyHeterodyne, MultiViewStereoGeometry, SinusShiftGrayCode, InterzoneSinusFourGrayscale, diff --git a/gui/src/CalibrateEngine.cpp b/gui/src/CalibrateEngine.cpp index 3ac8143..5666792 100644 --- a/gui/src/CalibrateEngine.cpp +++ b/gui/src/CalibrateEngine.cpp @@ -712,8 +712,7 @@ bool CalibrateEngine::captureOnce() { pattern->computePhaseMap(imgs, wrappedPhaseMap); pattern->computeFloorMap(imgs, confidenceMap, wrappedPhaseMap, floorMap); - pattern->unwrapPhaseMap(wrappedPhaseMap, floorMap, unwrapMap, - confidenceMap > params.confidenceThreshold); + pattern->unwrapPhaseMap(wrappedPhaseMap, floorMap, confidenceMap, unwrapMap); cv::normalize(unwrapMap, normalizeUnwrapMap, 0, 255, cv::NORM_MINMAX); diff --git a/gui/src/CameraEngine.cpp b/gui/src/CameraEngine.cpp index 4ef5942..93f2d02 100644 --- a/gui/src/CameraEngine.cpp +++ b/gui/src/CameraEngine.cpp @@ -69,7 +69,9 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction, const bool isKeepAdd) { qInfo() << "start create stripe..."; + int sinusImgsCount = 0; std::shared_ptr pattern; + if (stripeType == AppType::PatternMethod::SinusCompleGrayCode) { BinoSinusCompleGrayCodePattern::Params params; @@ -79,7 +81,21 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction, params.horizontal_ = direction == AppType::Direction::Horizion; params.shiftTime_ = shiftTime; + sinusImgsCount = params.shiftTime_; + pattern = BinoSinusCompleGrayCodePattern::create(params); + } else if (stripeType == AppType::PatternMethod::ThreeFrequencyHeterodyne) { + MonoThreeFrequencyHeterodynePattern::Params params; + + params.height_ = imgHeight; + params.width_ = imgWidth; + params.cycles_ = cycles; + params.horizontal_ = direction == AppType::Direction::Horizion; + params.shiftTime_ = shiftTime; + + sinusImgsCount = params.shiftTime_ * 3; + + pattern = MonoThreeFrequencyHeterodynePattern::create(params); } else if (stripeType == AppType::PatternMethod::MultiViewStereoGeometry) { TrinocularMultiViewStereoGeometryPattern::Params params; @@ -89,6 +105,8 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction, params.horizontal_ = direction == AppType::Direction::Horizion; params.shiftTime_ = shiftTime; + sinusImgsCount = params.shiftTime_; + pattern = TrinocularMultiViewStereoGeometryPattern::create(params); } else if (stripeType == AppType::PatternMethod::SinusShiftGrayCode) { MonoSinusShiftGrayCodePattern::Params params; @@ -99,8 +117,11 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction, params.horizontal_ = direction == AppType::Direction::Horizion; params.shiftTime_ = shiftTime; + sinusImgsCount = params.shiftTime_; + pattern = MonoSinusShiftGrayCodePattern::create(params); - } else if (stripeType == AppType::PatternMethod::InterzoneSinusFourGrayscale) { + } else if (stripeType == + AppType::PatternMethod::InterzoneSinusFourGrayscale) { MonoInterzoneSinusFourGrayscalePattern::Params params; params.height_ = imgHeight; @@ -109,6 +130,8 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction, params.horizontal_ = direction == AppType::Direction::Horizion; params.shiftTime_ = shiftTime; + sinusImgsCount = params.shiftTime_; + pattern = MonoInterzoneSinusFourGrayscalePattern::create(params); } @@ -116,7 +139,7 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction, pattern->generate(imgs); if (defocusMethod != AppType::DefocusEncoding::Disable) { - defocusStripeCreate(imgs, direction, cycles, shiftTime, + defocusStripeCreate(imgs, direction, cycles, sinusImgsCount, shiftTime, AppType::DefocusEncoding(defocusMethod)); } @@ -166,13 +189,14 @@ int CameraEngine::createStripe(const int pixelDepth, const int direction, void CameraEngine::defocusStripeCreate(std::vector &imgs, const int direction, const int cycles, + const int sinusImgsCount, const int shiftTime, AppType::DefocusEncoding method) { Q_ASSERT(!imgs.empty()); // TODO@Evans Liu: 使用浮点数相移图案会更精确点 for (int i = 0; i < imgs.size(); ++i) { - if (i < shiftTime) { + if (i < sinusImgsCount) { if (method == AppType::DefocusEncoding::ErrorDiffusionMethod) { twoDimensionErrorExpand(imgs[i]); } else if (method == AppType::DefocusEncoding::Binary) { @@ -184,9 +208,11 @@ void CameraEngine::defocusStripeCreate(std::vector &imgs, opwm(imgs[i], cycles, shiftVal, direction == AppType::Direction::Horizion); } - } else { - binary(imgs[i]); - } + + continue; + } + + binary(imgs[i]); } } @@ -646,8 +672,22 @@ void CameraEngine::setPatternType(const int patternType) { params.costMinDiff_ = costMinDiff; pattern_ = BinoSinusCompleGrayCodePattern::create(params); - } else if (patternType == AppType::PatternMethod::MutiplyFrequency) { + } else if (patternType == + AppType::PatternMethod::ThreeFrequencyHeterodyne) { + BinoThreeFrequencyHeterodynePattern::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_ = BinoThreeFrequencyHeterodynePattern::create(params); } else if (patternType == AppType::PatternMethod::SinusShiftGrayCode) { BinoSinusShiftGrayCodePattern::Params params; @@ -663,8 +703,8 @@ void CameraEngine::setPatternType(const int patternType) { params.costMinDiff_ = costMinDiff; pattern_ = BinoSinusShiftGrayCodePattern::create(params); - } - else if(patternType_ == AppType::PatternMethod::InterzoneSinusFourGrayscale) { + } else if (patternType_ == + AppType::PatternMethod::InterzoneSinusFourGrayscale) { BinoInterzoneSinusFourGrayscalePattern::Params params; params.shiftTime_ = std::round(shiftTime); @@ -740,10 +780,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::InterzoneSinusFourGrayscale) { + } else if (patternType == + AppType::PatternMethod::ThreeFrequencyHeterodyne) { + MonoThreeFrequencyHeterodynePattern::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_ = MonoThreeFrequencyHeterodynePattern::create(params); + } else if (patternType_ == + AppType::PatternMethod::InterzoneSinusFourGrayscale) { MonoInterzoneSinusFourGrayscalePattern::Params params; params.shiftTime_ = std::round(shiftTime); diff --git a/gui/src/CameraEngine.h b/gui/src/CameraEngine.h index 8689272..5b4dffa 100644 --- a/gui/src/CameraEngine.h +++ b/gui/src/CameraEngine.h @@ -123,7 +123,7 @@ class CameraEngine : public QObject { CameraEngine(const CameraEngine &) = delete; const CameraEngine &operator=(const CameraEngine &) = delete; void defocusStripeCreate(std::vector &imgs, const int direction, - const int cycles, const int shiftTime, + const int cycles, const int sinusImgsCount, const int shiftTime, AppType::DefocusEncoding method); void realTimeRenderImg(const QImage &img); void createTenLine(); diff --git a/perf/CMakeLists.txt b/perf/CMakeLists.txt index aa8eed9..f388839 100644 --- a/perf/CMakeLists.txt +++ b/perf/CMakeLists.txt @@ -46,4 +46,16 @@ target_link_libraries( PRIVATE benchmark::benchmark slmaster +) + +add_executable( + PerfThreeFrequencyHeterodynePattern + ${CMAKE_CURRENT_SOURCE_DIR}/perfThreeFrequencyHeterodynePattern.cpp +) + +target_link_libraries( + PerfThreeFrequencyHeterodynePattern + PRIVATE + benchmark::benchmark + slmaster ) \ No newline at end of file diff --git a/perf/perfInterzoneSinusFourGrayscalePattern.cpp b/perf/perfInterzoneSinusFourGrayscalePattern.cpp index e087fbb..d4d1c49 100644 --- a/perf/perfInterzoneSinusFourGrayscalePattern.cpp +++ b/perf/perfInterzoneSinusFourGrayscalePattern.cpp @@ -30,7 +30,7 @@ BENCHMARK_DEFINE_F(InterzoneSinusFourGrayscalePatternSuit, testGenerate) } } -BENCHMARK_DEFINE_F(InterzoneSinusFourGrayscalePatternSuit, testUnwrap) +BENCHMARK_DEFINE_F(InterzoneSinusFourGrayscalePatternSuit, testGenerateUnwrap) (benchmark::State &state) { auto params = InterzoneSinusFourGrayscalePattern::Params(); params.shiftTime = 3; @@ -56,6 +56,6 @@ BENCHMARK_DEFINE_F(InterzoneSinusFourGrayscalePatternSuit, testUnwrap) } BENCHMARK_REGISTER_F(InterzoneSinusFourGrayscalePatternSuit, testGenerate)->MeasureProcessCPUTime()->UseRealTime()->Unit(benchmark::TimeUnit::kMillisecond); -BENCHMARK_REGISTER_F(InterzoneSinusFourGrayscalePatternSuit, testUnwrap)->MeasureProcessCPUTime()->UseRealTime()->Unit(benchmark::TimeUnit::kMillisecond); +BENCHMARK_REGISTER_F(InterzoneSinusFourGrayscalePatternSuit, testGenerateUnwrap)->MeasureProcessCPUTime()->UseRealTime()->Unit(benchmark::TimeUnit::kMillisecond); BENCHMARK_MAIN(); \ No newline at end of file diff --git a/perf/perfShiftGrayCodePattern.cpp b/perf/perfShiftGrayCodePattern.cpp index ae396d6..b89c9d5 100644 --- a/perf/perfShiftGrayCodePattern.cpp +++ b/perf/perfShiftGrayCodePattern.cpp @@ -30,7 +30,7 @@ BENCHMARK_DEFINE_F(ShiftGrayCodePatternSuit, testGenerate) } } -BENCHMARK_DEFINE_F(ShiftGrayCodePatternSuit, testUnwrap) +BENCHMARK_DEFINE_F(ShiftGrayCodePatternSuit, testGenerateUnwrap) (benchmark::State &state) { auto params = SinusShiftGrayCodePattern::Params(); params.shiftTime = 4; @@ -50,7 +50,7 @@ BENCHMARK_DEFINE_F(ShiftGrayCodePatternSuit, testUnwrap) pattern->computePhaseMap(imgs, wrapPhaseMap); pattern->computeConfidenceMap(imgs, confidenceMap); pattern->computeFloorMap(imgs, confidenceMap, wrapPhaseMap, floorMap); - pattern->unwrapPhaseMap(wrapPhaseMap, floorMap, unwrapPhaseMap); + pattern->unwrapPhaseMap(wrapPhaseMap, floorMap, confidenceMap, unwrapPhaseMap); } } @@ -58,7 +58,7 @@ BENCHMARK_REGISTER_F(ShiftGrayCodePatternSuit, testGenerate) ->MeasureProcessCPUTime() ->UseRealTime() ->Unit(benchmark::TimeUnit::kMillisecond); -BENCHMARK_REGISTER_F(ShiftGrayCodePatternSuit, testUnwrap) +BENCHMARK_REGISTER_F(ShiftGrayCodePatternSuit, testGenerateUnwrap) ->MeasureProcessCPUTime() ->UseRealTime() ->Unit(benchmark::TimeUnit::kMillisecond); diff --git a/perf/perfThreeFrequencyHeterodynePattern.cpp b/perf/perfThreeFrequencyHeterodynePattern.cpp new file mode 100644 index 0000000..bdeb189 --- /dev/null +++ b/perf/perfThreeFrequencyHeterodynePattern.cpp @@ -0,0 +1,75 @@ +#include + +#include + +using namespace slmaster; +using namespace slmaster::algorithm; +using namespace std; +using namespace cv; + +const string imgsPath = "../../data/threeFrequencyHeterodyne/"; + +class ThreeFrequencyHeterodynePatternSuit : public benchmark::Fixture { + public: + void SetUp(const benchmark::State &) { + for (int i = 0; i < 12; ++i) { + Mat temp = imread(imgsPath + to_string(i) + ".bmp", 0); + imgs.emplace_back(temp); + } + } + + vector imgs; +}; + +BENCHMARK_DEFINE_F(ThreeFrequencyHeterodynePatternSuit, testGenerate)(benchmark::State& state) { + auto params = ThreeFrequencyHeterodynePattern::Params(); + params.shiftTime = 4; + params.height = 1080; + params.width = 1920; + params.horizontal = false; + params.nbrOfPeriods = 64; + + auto pattern = ThreeFrequencyHeterodynePattern::create(params); + + vector imgs; + + for (auto _ : state) { + pattern->generate(imgs); + } +} + +BENCHMARK_DEFINE_F(ThreeFrequencyHeterodynePatternSuit, testGenerateUnwrap)(benchmark::State& state) { + auto params = ThreeFrequencyHeterodynePattern::Params(); + params.shiftTime = 4; + params.height = 1080; + params.width = 1920; + params.horizontal = false; + params.nbrOfPeriods = 70; + + auto pattern = ThreeFrequencyHeterodynePattern::create(params); + + vector imgs; + pattern->generate(imgs); + + + for (auto _ : state) { + vector wrappedPhaseMaps; + Mat floorMap, confidenceMap, unwrapPhaseMap; + pattern->computePhaseMap(imgs, wrappedPhaseMaps); + pattern->computeConfidenceMap(imgs, confidenceMap); + pattern->computeFloorMap(wrappedPhaseMaps, confidenceMap, floorMap); + pattern->unwrapPhaseMap(wrappedPhaseMaps[0], floorMap, confidenceMap, + unwrapPhaseMap); + } +} + +BENCHMARK_REGISTER_F(ThreeFrequencyHeterodynePatternSuit, testGenerate) + ->MeasureProcessCPUTime() + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMillisecond); +BENCHMARK_REGISTER_F(ThreeFrequencyHeterodynePatternSuit, testGenerateUnwrap) + ->MeasureProcessCPUTime() + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMillisecond); + +BENCHMARK_MAIN(); \ No newline at end of file diff --git a/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.cpp b/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.cpp index ac8a056..e3280a1 100644 --- a/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.cpp +++ b/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.cpp @@ -24,8 +24,6 @@ class InterzoneSinusFourGrayscalePattern_Impl final // decode patterns and compute disparity map. bool decode(const std::vector> &patternImages, OutputArray disparityMap, - InputArrayOfArrays blackImages = noArray(), - InputArrayOfArrays whiteImages = noArray(), int flags = 0) const CV_OVERRIDE; // Compute a confidence map from sinusoidal patterns @@ -43,8 +41,8 @@ class InterzoneSinusFourGrayscalePattern_Impl final OutputArray floorMap) const CV_OVERRIDE; // Unwrap the wrapped phase map to remove phase ambiguities - void unwrapPhaseMap(cv::InputArray wrappedPhaseMap, - cv::InputArray confidenceMap, cv::InputArray floorMap, + void unwrapPhaseMap(cv::InputArray wrappedPhaseMap, cv::InputArray floorMap, + cv::InputArray confidenceMap, cv::OutputArray unwrappedPhaseMap) const CV_OVERRIDE; // Compute disparity @@ -53,8 +51,8 @@ class InterzoneSinusFourGrayscalePattern_Impl final private: // threshod four grayscale floor img - void threshod(const Mat &img, const Mat &confidenceMap, std::vector &threshodVal, - Mat &out) const; + void threshod(const Mat &img, const Mat &confidenceMap, + std::vector &threshodVal, Mat &out) const; // k-means cluster float kMeans(const Mat &img, const Mat &confidenceMap, std::vector &threshod) const; @@ -153,7 +151,7 @@ void InterzoneSinusFourGrayscalePattern_Impl::computePhaseMap( wrappedPhase = Mat::zeros(height, width, CV_32FC1); - const float shiftVal = static_cast(CV_2PI) / params.shiftTime; + const double shiftVal = static_cast(CV_2PI) / params.shiftTime; parallel_for_(Range(0, height), [&](const Range &range) { std::vector imgsPtrs(params.shiftTime); @@ -166,14 +164,19 @@ void InterzoneSinusFourGrayscalePattern_Impl::computePhaseMap( } for (int j = 0; j < width; ++j) { - float molecules = 0.f, denominator = 0.f; + double molecules = 0, denominator = 0; for (int k = 0; k < params.shiftTime; ++k) { molecules += imgsPtrs[k][j] * sin(k * shiftVal); denominator += imgsPtrs[k][j] * cos(k * shiftVal); } - wrappedPhasePtr[j] = -atan2(molecules, denominator); + float phase = -atan2(molecules, denominator); + + phase = phase > CV_PI ? 3.1415926 + : (phase < -CV_PI ? -3.1415926 : phase); + + wrappedPhasePtr[j] = phase; } } }); @@ -197,7 +200,8 @@ float InterzoneSinusFourGrayscalePattern_Impl::kMeans( for (int j = 0; j < img.cols; ++j) { // skip lower confidence pixels - if (ptrConfidenceMap[j] < params.confidenceThreshold || isnan(ptrImg[j])) { + if (ptrConfidenceMap[j] < params.confidenceThreshold || + isnan(ptrImg[j])) { continue; } // find minimum distance @@ -223,12 +227,12 @@ float InterzoneSinusFourGrayscalePattern_Impl::kMeans( for (int k = 0; k < 4; ++k) { 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) { + // 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; } @@ -239,7 +243,8 @@ float InterzoneSinusFourGrayscalePattern_Impl::kMeans( } void InterzoneSinusFourGrayscalePattern_Impl::threshod( - const Mat &img, const Mat &confidenceMap, std::vector &threshodVal, Mat &out) const { + const Mat &img, const Mat &confidenceMap, std::vector &threshodVal, + Mat &out) const { CV_Assert(!img.empty() && threshodVal.size() == 4); out = Mat::zeros(img.size(), CV_8UC1); @@ -255,7 +260,7 @@ void InterzoneSinusFourGrayscalePattern_Impl::threshod( auto ptrOut = out.ptr(i); for (int j = 0; j < img.cols; ++j) { - if(ptrConfidence[j] < params.confidenceThreshold) { + if (ptrConfidence[j] < params.confidenceThreshold) { continue; } @@ -295,7 +300,7 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeFloorMap( const int grayCodeImgsCount = imgs.size() - params.shiftTime; // compute texture Mat texture = Mat::zeros(confidence.size(), CV_32FC1); - parallel_for_(Range(0, params.height), [&](const Range &range) { + parallel_for_(Range(0, height), [&](const Range &range) { std::vector imgsPtrs(params.shiftTime); for (int i = range.start; i < range.end; ++i) { for (int j = 0; j < params.shiftTime; ++j) { @@ -304,7 +309,7 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeFloorMap( auto texturePtr = texture.ptr(i); - for (int j = 0; j < params.width; ++j) { + for (int j = 0; j < width; ++j) { for (auto ptr : imgsPtrs) { texturePtr[j] += ptr[j]; } @@ -342,7 +347,8 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeFloorMap( score = kMeans(normalizeGrayImgs[i], confidence, threshodVal); } while (++count < 3 && score > 0.01f); - threshod(normalizeGrayImgs[i], confidence, threshodVal, threshodGrayImgs[i]); + threshod(normalizeGrayImgs[i], confidence, threshodVal, + threshodGrayImgs[i]); } // compute floor map parallel_for_(Range(0, height), [&](const Range &range) { @@ -357,7 +363,7 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeFloorMap( auto ptrConfidence = confidence.ptr(i); for (int j = 0; j < width; ++j) { - if(ptrConfidence[j] < params.confidenceThreshold) { + if (ptrConfidence[j] < params.confidenceThreshold) { continue; } @@ -374,7 +380,7 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeFloorMap( } void InterzoneSinusFourGrayscalePattern_Impl::unwrapPhaseMap( - InputArray wrappedPhaseMap, InputArray confidenceMap, InputArray floorMap, + InputArray wrappedPhaseMap, InputArray floorMap, InputArray confidenceMap, OutputArray unwrappedPhaseMap) const { const Mat &wrappedPhase = *static_cast(wrappedPhaseMap.getObj()); @@ -449,16 +455,16 @@ bool InterzoneSinusFourGrayscalePattern_Impl::generate( // generate phase-shift imgs. for (int i = 0; i < params.shiftTime; ++i) { Mat intensityMap = Mat::zeros(height, width, CV_8UC1); - const float shiftVal = CV_2PI / params.shiftTime * i; + const double shiftVal = CV_2PI / params.shiftTime * i; for (int j = 0; j < height; ++j) { auto intensityMapPtr = intensityMap.ptr(j); for (int k = 0; k < width; ++k) { // Set the fringe starting intensity to 0 so that it corresponds // to the complementary graycode interval. - const float wrappedPhaseVal = + const double wrappedPhaseVal = (k % pixelsPerPeriod) / - static_cast(pixelsPerPeriod) * CV_2PI - + static_cast(pixelsPerPeriod) * CV_2PI - CV_PI; intensityMapPtr[k] = 127.5 + 127.5 * cos(wrappedPhaseVal + shiftVal); @@ -546,10 +552,7 @@ void InterzoneSinusFourGrayscalePattern_Impl::computeDisparity( bool InterzoneSinusFourGrayscalePattern_Impl::decode( const std::vector> &patternImages, - OutputArray disparityMap, InputArrayOfArrays blackImages, - InputArrayOfArrays whiteImages, int flags) const { - CV_UNUSED(blackImages); - CV_UNUSED(whiteImages); + OutputArray disparityMap, int flags) const { CV_Assert(!patternImages.empty()); @@ -572,8 +575,8 @@ bool InterzoneSinusFourGrayscalePattern_Impl::decode( computeFloorMap(patternImages[range.start], confidenceMap[range.start], floorMap[range.start]); // calculate unwrapped map - unwrapPhaseMap(wrappedMap[range.start], confidenceMap[range.start], - floorMap[range.start], unwrapMap[range.start]); + unwrapPhaseMap(wrappedMap[range.start], floorMap[range.start], + confidenceMap[range.start], unwrapMap[range.start]); }); // calculate disparity map diff --git a/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.hpp b/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.hpp index b276c5a..eef2be1 100644 --- a/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.hpp +++ b/src/algorithm/cpuStructuredLight/interzoneSinusFourGrayscalePattern.hpp @@ -100,14 +100,14 @@ class SLMASTER_API InterzoneSinusFourGrayscalePattern /** * @brief Unwrap the wrapped phase map to remove phase ambiguities. * @param wrappedPhaseMap The wrapped phase map computed from the pattern. - * @param confidenceThreshod confidence map. * @param floorMap floorMap map. + * @param confidenceThreshod confidence map. * @param unwrappedPhaseMap The unwrapped phase map used to find * correspondences between the two devices. */ virtual void unwrapPhaseMap(cv::InputArray wrappedPhaseMap, - cv::InputArray confidenceMap, cv::InputArray floorMap, + cv::InputArray confidenceMap, cv::OutputArray unwrappedPhaseMap) const = 0; /** * @brief compute disparity from left unwrap map and right unwrap map. diff --git a/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.cpp b/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.cpp index 15a3719..82e009f 100644 --- a/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.cpp +++ b/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.cpp @@ -22,8 +22,6 @@ class SinusCompleGrayCodePattern_Impl final // decode patterns and compute disparity map. bool decode(const std::vector> &patternImages, OutputArray disparityMap, - InputArrayOfArrays blackImages = noArray(), - InputArrayOfArrays whiteImages = noArray(), int flags = 0) const CV_OVERRIDE; // Compute a confidence map from sinusoidal patterns @@ -41,9 +39,8 @@ class SinusCompleGrayCodePattern_Impl final OutputArray floorMap) const CV_OVERRIDE; // Unwrap the wrapped phase map to remove phase ambiguities - void unwrapPhaseMap(InputArray wrappedPhaseMap, InputArray floorMap, - OutputArray unwrappedPhaseMap, - InputArray shadowMask = noArray()) const CV_OVERRIDE; + void unwrapPhaseMap(InputArray wrappedPhaseMap, InputArray floorMap, cv::InputArray confidenceMap, + OutputArray unwrappedPhaseMap) const CV_OVERRIDE; // Compute disparity void computeDisparity(InputArray lhsUnwrapMap, InputArray rhsUnwrapMap, @@ -80,34 +77,11 @@ void SinusCompleGrayCodePattern_Impl::computeConfidenceMap( confidence = Mat::zeros(imgs[0].size(), CV_32FC1); - const int height = imgs[0].rows; - const int width = imgs[0].cols; - - const float shiftVal = static_cast(CV_2PI) / params.shiftTime; - - parallel_for_(Range(0, height), [&](const Range &range) { - std::vector imgsPtrs(params.shiftTime); - - for (int i = range.start; i < range.end; ++i) { - auto confidencePtr = confidence.ptr(i); - - for (int j = 0; j < params.shiftTime; ++j) { - imgsPtrs[j] = imgs[j].ptr(i); - } - - for (int j = 0; j < width; ++j) { - float molecules = 0.f, denominator = 0.f; - for (int k = 0; k < params.shiftTime; ++k) { - molecules += imgsPtrs[k][j] * sin(k * shiftVal); - denominator += imgsPtrs[k][j] * cos(k * shiftVal); - } - - confidencePtr[j] = - 2.f / params.shiftTime * - sqrt(molecules * molecules + denominator * denominator); - } - } - }); + for (int i = 0; i < params.shiftTime; ++i) { + cv::Mat fltImg; + imgs[i].convertTo(fltImg, CV_32FC1); + confidence += fltImg / params.shiftTime; + } } void SinusCompleGrayCodePattern_Impl::computePhaseMap( @@ -122,7 +96,7 @@ void SinusCompleGrayCodePattern_Impl::computePhaseMap( const int width = imgs[0].cols; wrappedPhase = Mat::zeros(height, width, CV_32FC1); - const float shiftVal = static_cast(CV_2PI) / params.shiftTime; + const double shiftVal = static_cast(CV_2PI) / params.shiftTime; parallel_for_(Range(0, height), [&](const Range &range) { std::vector imgsPtrs(params.shiftTime); @@ -135,13 +109,17 @@ void SinusCompleGrayCodePattern_Impl::computePhaseMap( } for (int j = 0; j < width; ++j) { - float molecules = 0.f, denominator = 0.f; + double molecules = 0, denominator = 0; for (int k = 0; k < params.shiftTime; ++k) { molecules += imgsPtrs[k][j] * sin(k * shiftVal); denominator += imgsPtrs[k][j] * cos(k * shiftVal); } - wrappedPhasePtr[j] = -atan2(molecules, denominator); + float phase = -atan2(molecules, denominator); + + phase = phase > CV_PI ? 3.1415926 : (phase < -CV_PI ? -3.1415926 : phase); + + wrappedPhasePtr[j] = phase; } } }); @@ -207,11 +185,12 @@ void SinusCompleGrayCodePattern_Impl::computeFloorMap( } void SinusCompleGrayCodePattern_Impl::unwrapPhaseMap( - InputArray wrappedPhaseMap, InputArray floorMap, - OutputArray unwrappedPhaseMap, InputArray shadowMask) const { + InputArray wrappedPhaseMap, InputArray floorMap, cv::InputArray confidenceMap, + OutputArray unwrappedPhaseMap) const { const Mat &wrappedPhase = *static_cast(wrappedPhaseMap.getObj()); const Mat &floor = *static_cast(floorMap.getObj()); + const Mat &confidence = *static_cast(confidenceMap.getObj()); Mat &unwrappedPhase = *static_cast(unwrappedPhaseMap.getObj()); CV_Assert(!wrappedPhase.empty() && !floor.empty()); @@ -220,43 +199,22 @@ void SinusCompleGrayCodePattern_Impl::unwrapPhaseMap( const int width = wrappedPhase.cols; unwrappedPhase = Mat::zeros(height, width, CV_32FC1); - cv::Mat shadow; - if (!shadowMask.empty()) { - shadow = *static_cast(shadowMask.getObj()); - } - parallel_for_(Range(0, height), [&](const Range &range) { for (int i = range.start; i < range.end; ++i) { auto wrappedPhasePtr = wrappedPhase.ptr(i); auto floorPtr = floor.ptr(i); + auto confidencePtr = confidence.ptr(i); auto unwrappedPhasePtr = unwrappedPhase.ptr(i); - const uchar *shadowPtr = - shadow.empty() ? nullptr : shadow.ptr(i); + for (int j = 0; j < width; ++j) { // we add CV_PI to make wrap map to begin with 0. - if (shadowPtr) { - if (shadowPtr[j]) { - if (floorPtr[j] < params.nbrOfPeriods) { - unwrappedPhasePtr[j] = - wrappedPhasePtr[j] + - static_cast(CV_2PI) * floorPtr[j] + - static_cast(CV_PI); - } else { - unwrappedPhasePtr[j] = 0.f; - } - } else { - unwrappedPhasePtr[j] = 0.f; - } - } else { - if (floorPtr[j] < params.nbrOfPeriods) { - unwrappedPhasePtr[j] = - wrappedPhasePtr[j] + - static_cast(CV_2PI) * floorPtr[j] + - static_cast(CV_PI); - } else { - unwrappedPhasePtr[j] = 0.f; - } + if (confidencePtr[j] > params.confidenceThreshold) { + unwrappedPhasePtr[j] = + wrappedPhasePtr[j] + CV_2PI * floorPtr[j] + CV_PI; + continue; } + + unwrappedPhasePtr[j] = 0.f; } } }); @@ -271,19 +229,16 @@ bool SinusCompleGrayCodePattern_Impl::generate(OutputArrayOfArrays pattern) { // generate phase-shift imgs. for (int i = 0; i < params.shiftTime; ++i) { Mat intensityMap = Mat::zeros(height, width, CV_8UC1); - const float shiftVal = - static_cast(CV_2PI) / params.shiftTime * i; + const double shiftVal = CV_2PI / params.shiftTime * i; for (int j = 0; j < height; ++j) { auto intensityMapPtr = intensityMap.ptr(j); for (int k = 0; k < width; ++k) { // Set the fringe starting intensity to 0 so that it corresponds // to the complementary graycode interval. - const float wrappedPhaseVal = + const double wrappedPhaseVal = (k % pixelsPerPeriod) / - static_cast(pixelsPerPeriod) * - static_cast(CV_2PI) - - static_cast(CV_PI); + static_cast(pixelsPerPeriod) * CV_2PI - CV_PI; intensityMapPtr[k] = static_cast( 127.5 + 127.5 * cos(wrappedPhaseVal + shiftVal)); } @@ -333,10 +288,7 @@ void SinusCompleGrayCodePattern_Impl::computeDisparity( bool SinusCompleGrayCodePattern_Impl::decode( const std::vector> &patternImages, - OutputArray disparityMap, InputArrayOfArrays blackImages, - InputArrayOfArrays whiteImages, int flags) const { - CV_UNUSED(blackImages); - CV_UNUSED(whiteImages); + OutputArray disparityMap, int flags) const { CV_Assert(!patternImages.empty()); @@ -360,10 +312,8 @@ bool SinusCompleGrayCodePattern_Impl::decode( confidenceMap[range.start], wrappedMap[range.start], floorMap[range.start]); // calculate unwrapped map - unwrapPhaseMap(wrappedMap[range.start], floorMap[range.start], - unwrapMap[range.start], - confidenceMap[range.start] > - params.confidenceThreshold); + unwrapPhaseMap(wrappedMap[range.start], floorMap[range.start], confidenceMap[range.start], + unwrapMap[range.start]); }); // calculate disparity map diff --git a/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.hpp b/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.hpp index 42d95f4..8852060 100644 --- a/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.hpp +++ b/src/algorithm/cpuStructuredLight/sinusCompleGraycodePattern.hpp @@ -100,14 +100,14 @@ class SLMASTER_API SinusCompleGrayCodePattern : public StructuredLightPattern { /** * @brief Unwrap the wrapped phase map to remove phase ambiguities. * @param wrappedPhaseMap The wrapped phase map computed from the pattern. + * @param floorMap floor map. + * @param confidenceMap confidence map. * @param unwrappedPhaseMap The unwrapped phase map used to find * correspondences between the two devices. - * @param shadowMask Mask used to discard shadow regions. - * @param confidenceThreshod confidence threshod to discard invalid data. */ - virtual void unwrapPhaseMap(cv::InputArray wrappedPhaseMap, cv::InputArray floorMap, - cv::OutputArray unwrappedPhaseMap, - cv::InputArray shadowMask = cv::noArray()) const = 0; + virtual void + unwrapPhaseMap(cv::InputArray wrappedPhaseMap, cv::InputArray floorMap, cv::InputArray confidenceMap, + cv::OutputArray unwrappedPhaseMap) const = 0; /** * @brief compute disparity from left unwrap map and right unwrap map. * @param lhsUnwrapMap left unwrap map. diff --git a/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.cpp b/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.cpp index e82863d..6d2fb54 100644 --- a/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.cpp +++ b/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.cpp @@ -21,8 +21,6 @@ class SinusShiftGrayCodePattern_Impl final : public SinusShiftGrayCodePattern { // decode patterns and compute disparity map. bool decode(const std::vector> &patternImages, OutputArray disparityMap, - InputArrayOfArrays blackImages = noArray(), - InputArrayOfArrays whiteImages = noArray(), int flags = 0) const CV_OVERRIDE; // Compute a confidence map from sinusoidal patterns @@ -40,9 +38,8 @@ class SinusShiftGrayCodePattern_Impl final : public SinusShiftGrayCodePattern { OutputArray floorMap) const CV_OVERRIDE; // Unwrap the wrapped phase map to remove phase ambiguities - void unwrapPhaseMap(InputArray wrappedPhaseMap, InputArray floorMap, - OutputArray unwrappedPhaseMap, - InputArray shadowMask = noArray()) const CV_OVERRIDE; + void unwrapPhaseMap(InputArray wrappedPhaseMap, InputArray floorMap, InputArray confidenceMap, + OutputArray unwrappedPhaseMap) const CV_OVERRIDE; // Compute disparity void computeDisparity(InputArray lhsUnwrapMap, InputArray rhsUnwrapMap, @@ -79,34 +76,11 @@ void SinusShiftGrayCodePattern_Impl::computeConfidenceMap( confidence = Mat::zeros(imgs[0].size(), CV_32FC1); - const int height = imgs[0].rows; - const int width = imgs[0].cols; - - const float shiftVal = static_cast(CV_2PI) / params.shiftTime; - - parallel_for_(Range(0, height), [&](const Range &range) { - std::vector imgsPtrs(params.shiftTime); - - for (int i = range.start; i < range.end; ++i) { - auto confidencePtr = confidence.ptr(i); - - for (int j = 0; j < params.shiftTime; ++j) { - imgsPtrs[j] = imgs[j].ptr(i); - } - - for (int j = 0; j < width; ++j) { - float molecules = 0.f, denominator = 0.f; - for (int k = 0; k < params.shiftTime; ++k) { - molecules += imgsPtrs[k][j] * sin(k * shiftVal); - denominator += imgsPtrs[k][j] * cos(k * shiftVal); - } - - confidencePtr[j] = - 2.f / params.shiftTime * - sqrt(molecules * molecules + denominator * denominator); - } - } - }); + for (int i = 0; i < params.shiftTime; ++i) { + cv::Mat fltImg; + imgs[i].convertTo(fltImg, CV_32FC1); + confidence += fltImg / params.shiftTime; + } } void SinusShiftGrayCodePattern_Impl::computePhaseMap( @@ -121,7 +95,7 @@ void SinusShiftGrayCodePattern_Impl::computePhaseMap( const int width = imgs[0].cols; wrappedPhase = Mat::zeros(height, width, CV_32FC1); - const float shiftVal = static_cast(CV_2PI) / params.shiftTime; + const double shiftVal = static_cast(CV_2PI) / params.shiftTime; parallel_for_(Range(0, height), [&](const Range &range) { std::vector imgsPtrs(params.shiftTime); @@ -134,13 +108,18 @@ void SinusShiftGrayCodePattern_Impl::computePhaseMap( } for (int j = 0; j < width; ++j) { - float molecules = 0.f, denominator = 0.f; + double molecules = 0, denominator = 0; + for (int k = 0; k < params.shiftTime; ++k) { molecules += imgsPtrs[k][j] * sin(k * shiftVal); denominator += imgsPtrs[k][j] * cos(k * shiftVal); } - wrappedPhasePtr[j] = -atan2(molecules, denominator); + float phase = -atan2(molecules, denominator); + + phase = phase > CV_PI ? 3.1415926 : (phase < -CV_PI ? -3.1415926 : phase); + + wrappedPhasePtr[j] = phase; } } }); @@ -184,12 +163,7 @@ void SinusShiftGrayCodePattern_Impl::computeFloorMap( curK = (curK << 1) + tempVal; } - if (curK >= params.nbrOfPeriods) { - floorPtr[j] = 0; - continue; - } - - floorPtr[j] = curK; + floorPtr[j] = curK >= params.nbrOfPeriods ? 0 : curK; } } }); @@ -280,11 +254,12 @@ void SinusShiftGrayCodePattern_Impl::computeFloorMap( } void SinusShiftGrayCodePattern_Impl::unwrapPhaseMap( - InputArray wrappedPhaseMap, InputArray floorMap, - OutputArray unwrappedPhaseMap, InputArray shadowMask) const { + InputArray wrappedPhaseMap, InputArray floorMap, InputArray confidenceMap, + OutputArray unwrappedPhaseMap) const { const Mat &wrappedPhase = *static_cast(wrappedPhaseMap.getObj()); const Mat &floor = *static_cast(floorMap.getObj()); + const Mat &confidence = *static_cast(confidenceMap.getObj()); Mat &unwrappedPhase = *static_cast(unwrappedPhaseMap.getObj()); CV_Assert(!wrappedPhase.empty() && !floor.empty()); @@ -293,43 +268,21 @@ void SinusShiftGrayCodePattern_Impl::unwrapPhaseMap( const int width = wrappedPhase.cols; unwrappedPhase = Mat::zeros(height, width, CV_32FC1); - cv::Mat shadow; - if (!shadowMask.empty()) { - shadow = *static_cast(shadowMask.getObj()); - } - parallel_for_(Range(0, height), [&](const Range &range) { for (int i = range.start; i < range.end; ++i) { auto wrappedPhasePtr = wrappedPhase.ptr(i); auto floorPtr = floor.ptr(i); + auto confidencePtr = confidence.ptr(i); auto unwrappedPhasePtr = unwrappedPhase.ptr(i); - const uchar *shadowPtr = - shadow.empty() ? nullptr : shadow.ptr(i); for (int j = 0; j < width; ++j) { // we add CV_PI to make wrap map to begin with 0. - if (shadowPtr) { - if (shadowPtr[j]) { - if (floorPtr[j] < params.nbrOfPeriods) { - unwrappedPhasePtr[j] = - wrappedPhasePtr[j] + - static_cast(CV_2PI) * floorPtr[j] + - static_cast(CV_PI); - } else { - unwrappedPhasePtr[j] = 0.f; - } - } else { - unwrappedPhasePtr[j] = 0.f; - } - } else { - if (floorPtr[j] < params.nbrOfPeriods) { - unwrappedPhasePtr[j] = - wrappedPhasePtr[j] + - static_cast(CV_2PI) * floorPtr[j] + - static_cast(CV_PI); - } else { - unwrappedPhasePtr[j] = 0.f; - } + if (confidencePtr[j] > params.confidenceThreshold) { + unwrappedPhasePtr[j] = + wrappedPhasePtr[j] + CV_2PI * floorPtr[j] + CV_PI; + continue; } + + unwrappedPhasePtr[j] = 0.f; } } }); @@ -344,19 +297,16 @@ bool SinusShiftGrayCodePattern_Impl::generate(OutputArrayOfArrays pattern) { // generate phase-shift imgs. for (int i = 0; i < params.shiftTime; ++i) { Mat intensityMap = Mat::zeros(height, width, CV_8UC1); - const float shiftVal = - static_cast(CV_2PI) / params.shiftTime * i; + const double shiftVal = CV_2PI / params.shiftTime * i; for (int j = 0; j < height; ++j) { auto intensityMapPtr = intensityMap.ptr(j); for (int k = 0; k < width; ++k) { // Set the fringe starting intensity to 0 so that it corresponds // to the complementary graycode interval. - const float wrappedPhaseVal = + const double wrappedPhaseVal = (k % pixelsPerPeriod) / - static_cast(pixelsPerPeriod) * - static_cast(CV_2PI) - - static_cast(CV_PI); + static_cast(pixelsPerPeriod) * CV_2PI - CV_PI; intensityMapPtr[k] = static_cast( 127.5 + 127.5 * cos(wrappedPhaseVal + shiftVal)); } @@ -420,10 +370,7 @@ void SinusShiftGrayCodePattern_Impl::computeDisparity( bool SinusShiftGrayCodePattern_Impl::decode( const std::vector> &patternImages, - OutputArray disparityMap, InputArrayOfArrays blackImages, - InputArrayOfArrays whiteImages, int flags) const { - CV_UNUSED(blackImages); - CV_UNUSED(whiteImages); + OutputArray disparityMap, int flags) const { CV_Assert(!patternImages.empty()); @@ -447,10 +394,8 @@ bool SinusShiftGrayCodePattern_Impl::decode( confidenceMap[range.start], wrappedMap[range.start], floorMap[range.start]); // calculate unwrapped map - unwrapPhaseMap(wrappedMap[range.start], floorMap[range.start], - unwrapMap[range.start], - confidenceMap[range.start] > - params.confidenceThreshold); + unwrapPhaseMap(wrappedMap[range.start], floorMap[range.start], confidenceMap[range.start], + unwrapMap[range.start]); }); // calculate disparity map diff --git a/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.hpp b/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.hpp index b2dbeab..9d95120 100644 --- a/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.hpp +++ b/src/algorithm/cpuStructuredLight/sinusShiftGraycodePattern.hpp @@ -100,15 +100,14 @@ class SLMASTER_API SinusShiftGrayCodePattern : public StructuredLightPattern { /** * @brief Unwrap the wrapped phase map to remove phase ambiguities. * @param wrappedPhaseMap The wrapped phase map computed from the pattern. + * @param floorMap floor map. + * @param confidenceMap confidence map. * @param unwrappedPhaseMap The unwrapped phase map used to find * correspondences between the two devices. - * @param shadowMask Mask used to discard shadow regions. - * @param confidenceThreshod confidence threshod to discard invalid data. */ virtual void - unwrapPhaseMap(cv::InputArray wrappedPhaseMap, cv::InputArray floorMap, - cv::OutputArray unwrappedPhaseMap, - cv::InputArray shadowMask = cv::noArray()) const = 0; + unwrapPhaseMap(cv::InputArray wrappedPhaseMap, cv::InputArray floorMap, cv::InputArray confidenceMap, + cv::OutputArray unwrappedPhaseMap) const = 0; /** * @brief compute disparity from left unwrap map and right unwrap map. * @param lhsUnwrapMap left unwrap map. diff --git a/src/algorithm/cpuStructuredLight/structuredLight.hpp b/src/algorithm/cpuStructuredLight/structuredLight.hpp index adae511..25877bf 100644 --- a/src/algorithm/cpuStructuredLight/structuredLight.hpp +++ b/src/algorithm/cpuStructuredLight/structuredLight.hpp @@ -62,8 +62,6 @@ class SLMASTER_API StructuredLightPattern : public virtual cv::Algorithm { (vector>), loaded as grayscale and previously rectified. @param disparityMap The decoding result: a CV_64F Mat at image resolution, storing the computed disparity map. - @param blackImages The all-black images needed for shadowMasks computation. - @param whiteImages The all-white images needed for shadowMasks computation. @param flags Flags setting decoding algorithms. Default: DECODE_3D_UNDERWORLD. @note All the images must be at the same resolution. @@ -71,8 +69,6 @@ class SLMASTER_API StructuredLightPattern : public virtual cv::Algorithm { virtual bool decode(const std::vector> &patternImages, cv::OutputArray disparityMap, - cv::InputArrayOfArrays blackImages = cv::noArray(), - cv::InputArrayOfArrays whiteImages = cv::noArray(), int flags = SINUSOIDAL_COMPLEMENTARY_GRAY_CODE) const = 0; }; diff --git a/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.cpp b/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.cpp index a52b0ac..1de1726 100644 --- a/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.cpp +++ b/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.cpp @@ -22,8 +22,6 @@ class ThreeFrequencyHeterodynePattern_Impl final // decode patterns and compute disparity map. bool decode(const std::vector> &patternImages, OutputArray disparityMap, - InputArrayOfArrays blackImages = noArray(), - InputArrayOfArrays whiteImages = noArray(), int flags = 0) const CV_OVERRIDE; // Compute a confidence map from sinusoidal patterns @@ -42,8 +40,8 @@ class ThreeFrequencyHeterodynePattern_Impl final // Unwrap the wrapped phase map to remove phase ambiguities void unwrapPhaseMap(InputArray wrappedPhaseMap, InputArray floorMap, - OutputArray unwrappedPhaseMap, - InputArray shadowMask = noArray()) const CV_OVERRIDE; + InputArray confidenceMap, + OutputArray unwrappedPhaseMap) const CV_OVERRIDE; // Compute disparity void computeDisparity(InputArray lhsUnwrapMap, InputArray rhsUnwrapMap, @@ -128,7 +126,7 @@ void ThreeFrequencyHeterodynePattern_Impl::computePhaseMap( wrappedPhase[i] = Mat::zeros(height, width, CV_32FC1); } - const float shiftVal = CV_2PI / params.shiftTime; + const double shiftVal = CV_2PI / params.shiftTime; parallel_for_(Range(0, height), [&](const Range &range) { std::vector imgsPtrs(imgs.size()); @@ -145,14 +143,21 @@ void ThreeFrequencyHeterodynePattern_Impl::computePhaseMap( for (int j = 0; j < width; ++j) { for (int k = 0; k < 3; ++k) { - float molecules = 0.f, denominator = 0.f; + double molecules = 0, denominator = 0; for (int s = 0; s < params.shiftTime; ++s) { - molecules += imgsPtrs[s + params.shiftTime * k][j] * sin(s * shiftVal); - denominator += - imgsPtrs[s + params.shiftTime * k][j] * cos(s * shiftVal); + molecules += imgsPtrs[s + params.shiftTime * k][j] * + sin(s * shiftVal); + denominator += imgsPtrs[s + params.shiftTime * k][j] * + cos(s * shiftVal); } - wrappedPhasesPtr[k][j] = -atan2(molecules, denominator); + float phase = -atan2(molecules, denominator); + + phase = phase > CV_PI + ? 3.1415926 + : (phase < -CV_PI ? -3.1415926 : phase); + + wrappedPhasesPtr[k][j] = phase; } } } @@ -173,17 +178,11 @@ void ThreeFrequencyHeterodynePattern_Impl::computeFloorMap( const int width = confidence.cols; floor = Mat::zeros(height, width, CV_16UC1); - std::vector frequencies = { 1.f / params.nbrOfPeriods, 1.f / (params.nbrOfPeriods - 6), 1.f / (params.nbrOfPeriods - 11)}; + std::vector weeks = {params.nbrOfPeriods, params.nbrOfPeriods - 6, + params.nbrOfPeriods - 11}; - float frequency12 = frequencies[0] * frequencies[1] / (frequencies[1] - frequencies[0]); - float frequency23 = frequencies[1] * frequencies[2] / (frequencies[2] - frequencies[1]); - float frequency123 = frequency23 * frequency12 / (frequency23 - frequency12); - float ratio2d1 = frequency12 / frequencies[0]; - float ratio123d12 = frequency123 / frequency12; - - Mat wrap12 = Mat::zeros(height, width, CV_32FC1); - Mat wrap23 = Mat::zeros(height, width, CV_32FC1); - Mat wrap123 = Mat::zeros(height, width, CV_32FC1); + float ratio123d12 = 6.f; + float ratio2d1 = params.nbrOfPeriods / 6.f; parallel_for_(Range(0, height), [&](const Range &range) { std::vector wrapMapsPtrs(3); @@ -197,31 +196,38 @@ void ThreeFrequencyHeterodynePattern_Impl::computeFloorMap( auto floorPtr = floor.ptr(i); for (int j = 0; j < width; ++j) { + if(confidencePtr[j] < params.confidenceThreshold) { + continue; + } + float p12 = wrapMapsPtrs[0][j] - wrapMapsPtrs[1][j]; p12 += p12 < 0 ? CV_2PI : 0; float p23 = wrapMapsPtrs[1][j] - wrapMapsPtrs[2][j]; p23 += p23 < 0 ? CV_2PI : 0; float p123 = p12 - p23; p123 += p123 < 0 ? CV_2PI : 0; - auto floor12 = static_cast(round((p123 * ratio123d12 - p12) / CV_2PI) + 0.1); + + int floor12 = round((p123 * ratio123d12 - p12) / CV_2PI); + floor12 = floor12 == -1 ? ratio123d12 - 1 : floor12; auto unwrap12 = p12 + floor12 * CV_2PI; - auto floor1 = static_cast(round((unwrap12 * ratio2d1 - wrapMapsPtrs[0][j]) / CV_2PI) + 0.1); - floorPtr[j] = floor1; + int floor1 = + round((unwrap12 * ratio2d1 - wrapMapsPtrs[0][j] - CV_PI) / + CV_2PI); + floor1 = floor1 >= params.nbrOfPeriods ? 0 : floor1; - wrap12.ptr(i)[j] = p12; - wrap23.ptr(i)[j] = p23; - wrap123.ptr(i)[j] = p123; + floorPtr[j] = floor1; } } }); } void ThreeFrequencyHeterodynePattern_Impl::unwrapPhaseMap( - InputArray wrappedPhaseMap, InputArray floorMap, - OutputArray unwrappedPhaseMap, InputArray shadowMask) const { + InputArray wrappedPhaseMap, InputArray floorMap, InputArray confidenceMap, + OutputArray unwrappedPhaseMap) const { const Mat &wrappedPhase = *static_cast(wrappedPhaseMap.getObj()); const Mat &floor = *static_cast(floorMap.getObj()); + const Mat &confidence = *static_cast(confidenceMap.getObj()); Mat &unwrappedPhase = *static_cast(unwrappedPhaseMap.getObj()); CV_Assert(!wrappedPhase.empty() && !floor.empty()); @@ -230,43 +236,21 @@ void ThreeFrequencyHeterodynePattern_Impl::unwrapPhaseMap( const int width = wrappedPhase.cols; unwrappedPhase = Mat::zeros(height, width, CV_32FC1); - cv::Mat shadow; - if (!shadowMask.empty()) { - shadow = *static_cast(shadowMask.getObj()); - } - parallel_for_(Range(0, height), [&](const Range &range) { for (int i = range.start; i < range.end; ++i) { auto wrappedPhasePtr = wrappedPhase.ptr(i); auto floorPtr = floor.ptr(i); + auto confidencePtr = confidence.ptr(i); auto unwrappedPhasePtr = unwrappedPhase.ptr(i); - const uchar *shadowPtr = - shadow.empty() ? nullptr : shadow.ptr(i); for (int j = 0; j < width; ++j) { // we add CV_PI to make wrap map to begin with 0. - if (shadowPtr) { - if (shadowPtr[j]) { - if (floorPtr[j] < params.nbrOfPeriods) { - unwrappedPhasePtr[j] = - wrappedPhasePtr[j] + - static_cast(CV_2PI) * floorPtr[j] + - static_cast(CV_PI); - } else { - unwrappedPhasePtr[j] = 0.f; - } - } else { - unwrappedPhasePtr[j] = 0.f; - } - } else { - if (floorPtr[j] < params.nbrOfPeriods) { - unwrappedPhasePtr[j] = - wrappedPhasePtr[j] + - static_cast(CV_2PI) * floorPtr[j] + - static_cast(CV_PI); - } else { - unwrappedPhasePtr[j] = 0.f; - } + if (confidencePtr[j] > params.confidenceThreshold) { + unwrappedPhasePtr[j] = + wrappedPhasePtr[j] + CV_2PI * floorPtr[j] + CV_PI; + continue; } + + unwrappedPhasePtr[j] = 0.f; } } }); @@ -278,16 +262,15 @@ bool ThreeFrequencyHeterodynePattern_Impl::generate( imgs.clear(); const int height = params.horizontal ? params.width : params.height; const int width = params.horizontal ? params.height : params.width; - std::vector weeks = { params.nbrOfPeriods, params.nbrOfPeriods - 6, params.nbrOfPeriods - 11}; + std::vector weeks = {params.nbrOfPeriods, params.nbrOfPeriods - 6, + params.nbrOfPeriods - 11}; // generate phase-shift imgs. for (int s = 0; s < 3; ++s) { - //can be float point - const float pixelsPerPeriod = - static_cast(width) / weeks[s]; + // can be float point + const float pixelsPerPeriod = static_cast(width) / weeks[s]; for (int i = 0; i < params.shiftTime; ++i) { - const float shiftVal = - static_cast(CV_2PI) / params.shiftTime * i; + const float shiftVal = CV_2PI / params.shiftTime * i; Mat intensityMap = Mat::zeros(height, width, CV_8UC1); @@ -296,10 +279,9 @@ bool ThreeFrequencyHeterodynePattern_Impl::generate( for (int k = 0; k < width; ++k) { // Set the fringe starting intensity to 0 so that it // corresponds to the complementary graycode interval. - const float wrappedPhaseVal = - ((k / pixelsPerPeriod) - floor(k / pixelsPerPeriod)) * - CV_2PI - - CV_PI; + const float denom = + k / pixelsPerPeriod - floor(k / pixelsPerPeriod); + const float wrappedPhaseVal = (2 * denom - 1) * CV_PI; intensityMapPtr[k] = static_cast( 127.5 + 127.5 * cos(wrappedPhaseVal + shiftVal)); } @@ -328,10 +310,7 @@ void ThreeFrequencyHeterodynePattern_Impl::computeDisparity( bool ThreeFrequencyHeterodynePattern_Impl::decode( const std::vector> &patternImages, - OutputArray disparityMap, InputArrayOfArrays blackImages, - InputArrayOfArrays whiteImages, int flags) const { - CV_UNUSED(blackImages); - CV_UNUSED(whiteImages); + OutputArray disparityMap, int flags) const { CV_Assert(!patternImages.empty()); @@ -355,9 +334,7 @@ bool ThreeFrequencyHeterodynePattern_Impl::decode( floorMap[range.start]); // calculate unwrapped map unwrapPhaseMap(wrappedMap[range.start], floorMap[range.start], - unwrapMap[range.start], - confidenceMap[range.start] > - params.confidenceThreshold); + confidenceMap[range.start], unwrapMap[range.start]); }); // calculate disparity map diff --git a/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.hpp b/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.hpp index abd824e..acfecfb 100644 --- a/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.hpp +++ b/src/algorithm/cpuStructuredLight/threeFrequencyHeterodynePattern.hpp @@ -91,14 +91,13 @@ class SLMASTER_API ThreeFrequencyHeterodynePattern : public StructuredLightPatte /** * @brief Unwrap the wrapped phase map to remove phase ambiguities. * @param wrappedPhaseMap The wrapped phase map computed from the pattern. + * @param floorMap floor map. + * @param confidenceMap confidence map. * @param unwrappedPhaseMap The unwrapped phase map used to find * correspondences between the two devices. - * @param shadowMask Mask used to discard shadow regions. - * @param confidenceThreshod confidence threshod to discard invalid data. */ virtual void unwrapPhaseMap(cv::InputArray wrappedPhaseMap, cv::InputArray floorMap, - cv::OutputArray unwrappedPhaseMap, - cv::InputArray shadowMask = cv::noArray()) const = 0; + cv::InputArray confidenceMap, cv::OutputArray unwrappedPhaseMap) const = 0; /** * @brief compute disparity from left unwrap map and right unwrap map. * @param lhsUnwrapMap left unwrap map. diff --git a/src/algorithm/gpuStructuredLight/cudaSinusCompleGraycodePattern.cpp b/src/algorithm/gpuStructuredLight/cudaSinusCompleGraycodePattern.cpp index 2139714..b564009 100644 --- a/src/algorithm/gpuStructuredLight/cudaSinusCompleGraycodePattern.cpp +++ b/src/algorithm/gpuStructuredLight/cudaSinusCompleGraycodePattern.cpp @@ -90,8 +90,7 @@ bool SinusCompleGrayCodePatternGPU_Impl::generate(std::vector &pattern) // generate phase-shift imgs. for (int i = 0; i < params.shiftTime; ++i) { Mat intensityMap = Mat::zeros(height, width, CV_8UC1); - const float shiftVal = - static_cast(CV_2PI) / params.shiftTime * i; + const double shiftVal = CV_2PI / params.shiftTime * i; for (int j = 0; j < height; ++j) { auto intensityMapPtr = intensityMap.ptr(j); @@ -100,9 +99,7 @@ bool SinusCompleGrayCodePatternGPU_Impl::generate(std::vector &pattern) // to the complementary graycode interval. const float wrappedPhaseVal = (k % pixelsPerPeriod) / - static_cast(pixelsPerPeriod) * - static_cast(CV_2PI) - - static_cast(CV_PI); + static_cast(pixelsPerPeriod) * CV_2PI - CV_PI; intensityMapPtr[k] = static_cast( 127.5 + 127.5 * cos(wrappedPhaseVal + shiftVal)); } diff --git a/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.cpp b/src/cameras/binocular/binoInterzoneSinusFourGrayscalePattern.cpp similarity index 84% rename from src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.cpp rename to src/cameras/binocular/binoInterzoneSinusFourGrayscalePattern.cpp index cfe7814..9313180 100644 --- a/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.cpp +++ b/src/cameras/binocular/binoInterzoneSinusFourGrayscalePattern.cpp @@ -1,4 +1,4 @@ -#include "binosInterzoneSinusFourGrayscalePattern.h" +#include "binoInterzoneSinusFourGrayscalePattern.h" #include "../algorithm/algorithm.h" @@ -9,7 +9,8 @@ namespace cameras { static BinoInterzoneSinusFourGrayscalePattern::Params params__; -BinoInterzoneSinusFourGrayscalePattern::BinoInterzoneSinusFourGrayscalePattern() {} +BinoInterzoneSinusFourGrayscalePattern:: + BinoInterzoneSinusFourGrayscalePattern() {} bool BinoInterzoneSinusFourGrayscalePattern::generate( std::vector &imgs) const { @@ -28,7 +29,8 @@ bool BinoInterzoneSinusFourGrayscalePattern::generate( ->generate(imgs); } -std::shared_ptr BinoInterzoneSinusFourGrayscalePattern::create(const Params& params) { +std::shared_ptr +BinoInterzoneSinusFourGrayscalePattern::create(const Params ¶ms) { params__ = params; return std::make_shared(); @@ -73,10 +75,10 @@ bool BinoInterzoneSinusFourGrayscalePattern::decode( params.maxDisparity = params__.maxDisparity_; params.confidenceThreshold = params__.confidenceThreshold_; - auto pattern = algorithm::InterzoneSinusFourGrayscalePattern::create(params); - return pattern->decode( - patternImages, disparityMap, cv::noArray(), cv::noArray(), - algorithm::INTERZONE_SINUS_FOUR_GRAYSCALE); + auto pattern = + algorithm::InterzoneSinusFourGrayscalePattern::create(params); + return pattern->decode(patternImages, disparityMap, + algorithm::INTERZONE_SINUS_FOUR_GRAYSCALE); } } // namespace cameras } // namespace slmaster \ No newline at end of file diff --git a/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.h b/src/cameras/binocular/binoInterzoneSinusFourGrayscalePattern.h similarity index 88% rename from src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.h rename to src/cameras/binocular/binoInterzoneSinusFourGrayscalePattern.h index fbd904d..12243a7 100644 --- a/src/cameras/binocular/binosInterzoneSinusFourGrayscalePattern.h +++ b/src/cameras/binocular/binoInterzoneSinusFourGrayscalePattern.h @@ -9,8 +9,8 @@ * */ -#ifndef __BINOS_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_ -#define __BINOS_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_ +#ifndef __BINO_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_ +#define __BINO_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_ #include "../../common.h" #include "../pattern.h" @@ -47,4 +47,4 @@ class SLMASTER_API BinoInterzoneSinusFourGrayscalePattern : public Pattern { } // namespace cameras } // namespace slmaster -#endif // __BINOS_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_ \ No newline at end of file +#endif // __BINO_INTERZONE_SINUS_FOUR_GRAYSCALE_PATTERN_H_ \ No newline at end of file diff --git a/src/cameras/binocular/binosSinusCompleGrayCodePattern.cpp b/src/cameras/binocular/binoSinusCompleGrayCodePattern.cpp similarity index 84% rename from src/cameras/binocular/binosSinusCompleGrayCodePattern.cpp rename to src/cameras/binocular/binoSinusCompleGrayCodePattern.cpp index 7b33a01..1cfb1c0 100644 --- a/src/cameras/binocular/binosSinusCompleGrayCodePattern.cpp +++ b/src/cameras/binocular/binoSinusCompleGrayCodePattern.cpp @@ -1,4 +1,4 @@ -#include "binosSinusCompleGrayCodePattern.h" +#include "binoSinusCompleGrayCodePattern.h" #include "../../algorithm/algorithm.h" @@ -24,11 +24,12 @@ bool BinoSinusCompleGrayCodePattern::generate( params.confidenceThreshold = params__.confidenceThreshold_; params.shiftTime = params__.shiftTime_; - return algorithm::SinusCompleGrayCodePattern::create(params) - ->generate(imgs); + return algorithm::SinusCompleGrayCodePattern::create(params)->generate( + imgs); } -std::shared_ptr BinoSinusCompleGrayCodePattern::create(const Params& params) { +std::shared_ptr +BinoSinusCompleGrayCodePattern::create(const Params ¶ms) { params__ = params; return std::make_shared(); @@ -54,8 +55,9 @@ bool BinoSinusCompleGrayCodePattern::decode( params.maxDisparity = params__.maxDisparity_; auto pattern = algorithm::SinusCompleGrayCodePatternGPU::create(params); - return pattern->decode(patternImages, disparityMap, - algorithm::SINUSOIDAL_COMPLEMENTARY_GRAY_CODE_GPU); + return pattern->decode( + patternImages, disparityMap, + algorithm::SINUSOIDAL_COMPLEMENTARY_GRAY_CODE_GPU); } #endif @@ -71,9 +73,8 @@ bool BinoSinusCompleGrayCodePattern::decode( params.confidenceThreshold = params__.confidenceThreshold_; auto pattern = algorithm::SinusCompleGrayCodePattern::create(params); - return pattern->decode( - patternImages, disparityMap, cv::noArray(), cv::noArray(), - algorithm::SINUSOIDAL_COMPLEMENTARY_GRAY_CODE); + return pattern->decode(patternImages, disparityMap, + algorithm::SINUSOIDAL_COMPLEMENTARY_GRAY_CODE); } } // namespace cameras } // namespace slmaster \ No newline at end of file diff --git a/src/cameras/binocular/binosSinusCompleGrayCodePattern.h b/src/cameras/binocular/binoSinusCompleGrayCodePattern.h similarity index 90% rename from src/cameras/binocular/binosSinusCompleGrayCodePattern.h rename to src/cameras/binocular/binoSinusCompleGrayCodePattern.h index 0700413..72ae77d 100644 --- a/src/cameras/binocular/binosSinusCompleGrayCodePattern.h +++ b/src/cameras/binocular/binoSinusCompleGrayCodePattern.h @@ -9,8 +9,8 @@ * */ -#ifndef __BINOS_SINUS_COMPLE_GRAYCODE_PATTERN_H_ -#define __BINOS_SINUS_COMPLE_GRAYCODE_PATTERN_H_ +#ifndef __BINO_SINUS_COMPLE_GRAYCODE_PATTERN_H_ +#define __BINO_SINUS_COMPLE_GRAYCODE_PATTERN_H_ #include "../../common.h" #include "../pattern.h" @@ -49,4 +49,4 @@ class SLMASTER_API BinoSinusCompleGrayCodePattern : public Pattern { } // namespace cameras } // namespace slmaster -#endif // __BINOS_SINUS_COMPLE_GRAYCODE_PATTERN_H_ +#endif // __BINO_SINUS_COMPLE_GRAYCODE_PATTERN_H_ diff --git a/src/cameras/binocular/binosSinusShiftGrayCodePattern.cpp b/src/cameras/binocular/binoSinusShiftGrayCodePattern.cpp similarity index 84% rename from src/cameras/binocular/binosSinusShiftGrayCodePattern.cpp rename to src/cameras/binocular/binoSinusShiftGrayCodePattern.cpp index d11f22a..b49d0c0 100644 --- a/src/cameras/binocular/binosSinusShiftGrayCodePattern.cpp +++ b/src/cameras/binocular/binoSinusShiftGrayCodePattern.cpp @@ -1,4 +1,4 @@ -#include "binosSinusShiftGrayCodePattern.h" +#include "binoSinusShiftGrayCodePattern.h" #include "../../algorithm/algorithm.h" @@ -11,8 +11,7 @@ static BinoSinusShiftGrayCodePattern::Params params__; BinoSinusShiftGrayCodePattern::BinoSinusShiftGrayCodePattern() {} -bool BinoSinusShiftGrayCodePattern::generate( - std::vector &imgs) const { +bool BinoSinusShiftGrayCodePattern::generate(std::vector &imgs) const { algorithm::SinusShiftGrayCodePattern::Params params; params.width = params__.width_; params.height = params__.height_; @@ -24,11 +23,11 @@ bool BinoSinusShiftGrayCodePattern::generate( params.confidenceThreshold = params__.confidenceThreshold_; params.shiftTime = params__.shiftTime_; - return algorithm::SinusShiftGrayCodePattern::create(params) - ->generate(imgs); + return algorithm::SinusShiftGrayCodePattern::create(params)->generate(imgs); } -std::shared_ptr BinoSinusShiftGrayCodePattern::create(const Params& params) { +std::shared_ptr +BinoSinusShiftGrayCodePattern::create(const Params ¶ms) { params__ = params; return std::make_shared(); @@ -74,9 +73,8 @@ bool BinoSinusShiftGrayCodePattern::decode( params.confidenceThreshold = params__.confidenceThreshold_; auto pattern = algorithm::SinusShiftGrayCodePattern::create(params); - return pattern->decode( - patternImages, disparityMap, cv::noArray(), cv::noArray(), - algorithm::SINUSOIDAL_SHIFT_GRAY_CODE); + return pattern->decode(patternImages, disparityMap, + algorithm::SINUSOIDAL_SHIFT_GRAY_CODE); } } // namespace cameras } // namespace slmaster \ No newline at end of file diff --git a/src/cameras/binocular/binosSinusShiftGrayCodePattern.h b/src/cameras/binocular/binoSinusShiftGrayCodePattern.h similarity index 89% rename from src/cameras/binocular/binosSinusShiftGrayCodePattern.h rename to src/cameras/binocular/binoSinusShiftGrayCodePattern.h index 5c4c5a1..f80ed93 100644 --- a/src/cameras/binocular/binosSinusShiftGrayCodePattern.h +++ b/src/cameras/binocular/binoSinusShiftGrayCodePattern.h @@ -9,8 +9,8 @@ * */ -#ifndef __BINOS_SINUS_SHIFT_GRAYCODE_PATTERN_H_ -#define __BINOS_SINUS_SHIFT_GRAYCODE_PATTERN_H_ +#ifndef __BINO_SINUS_SHIFT_GRAYCODE_PATTERN_H_ +#define __BINO_SINUS_SHIFT_GRAYCODE_PATTERN_H_ #include "../../common.h" #include "../pattern.h" @@ -47,4 +47,4 @@ class SLMASTER_API BinoSinusShiftGrayCodePattern : public Pattern { } // namespace cameras } // namespace slmaster -#endif // __BINOS_SINUS_SHIFT_GRAYCODE_PATTERN_H_ +#endif // __BINO_SINUS_SHIFT_GRAYCODE_PATTERN_H_ diff --git a/src/cameras/binocular/binoThreeFrequencyHeterodynePattern.cpp b/src/cameras/binocular/binoThreeFrequencyHeterodynePattern.cpp new file mode 100644 index 0000000..62a4913 --- /dev/null +++ b/src/cameras/binocular/binoThreeFrequencyHeterodynePattern.cpp @@ -0,0 +1,64 @@ +#include "binoThreeFrequencyHeterodynePattern.h" + +#include "../../algorithm/algorithm.h" + +using namespace cv; + +namespace slmaster { +namespace cameras { + +static BinoThreeFrequencyHeterodynePattern::Params params__; + +BinoThreeFrequencyHeterodynePattern::BinoThreeFrequencyHeterodynePattern() {} + +bool BinoThreeFrequencyHeterodynePattern::generate( + std::vector &imgs) const { + algorithm::ThreeFrequencyHeterodynePattern::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::ThreeFrequencyHeterodynePattern::create(params)->generate( + imgs); +} + +std::shared_ptr +BinoThreeFrequencyHeterodynePattern::create(const Params ¶ms) { + params__ = params; + + return std::make_shared(); +} + +bool BinoThreeFrequencyHeterodynePattern::decode( + const std::vector> &patternImages, + cv::Mat &disparityMap, const bool isGpu) const { + CV_Assert(patternImages.size() >= 2); + +#ifdef OPENCV_WITH_CUDA_MODULE + if (isGpu) { + } +#endif + + algorithm::ThreeFrequencyHeterodynePattern::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::ThreeFrequencyHeterodynePattern::create(params); + return pattern->decode(patternImages, disparityMap, + algorithm::THREE_FREQUENCY_HETERODYNE); +} +} // namespace cameras +} // namespace slmaster \ No newline at end of file diff --git a/src/cameras/binocular/binoThreeFrequencyHeterodynePattern.h b/src/cameras/binocular/binoThreeFrequencyHeterodynePattern.h new file mode 100644 index 0000000..6b98e61 --- /dev/null +++ b/src/cameras/binocular/binoThreeFrequencyHeterodynePattern.h @@ -0,0 +1,52 @@ +/** + * @file binoThreeFrequencyHeterodynePattern.h + * @author Evans Liu (1369215984@qq.com) + * @brief + * @version 0.1 + * @date 2024-04-17 + * + * @copyright Copyright (c) 2024 + * + */ + +#ifndef __BINO_THREE_FREQUENCY_HETERODYNE_PATTERN_H_ +#define __BINO_THREE_FREQUENCY_HETERODYNE_PATTERN_H_ + +#include "../../common.h" +#include "../pattern.h" + +namespace slmaster { +namespace cameras { +// TODO@Evans +// Liu:使用修饰器模式会更好，避免未来方法增多导致的子类爆炸，需要在OpenCV中重新更改接口 +class SLMASTER_API BinoThreeFrequencyHeterodynePattern : public Pattern { + public: + struct SLMASTER_API Params { + Params() + : width_(1920), height_(1080), shiftTime_(4), cycles_(32), + 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_; + }; + + BinoThreeFrequencyHeterodynePattern(); + virtual bool generate(IN std::vector &imgs) const override final; + virtual bool + decode(IN const std::vector> &patternImages, + OUT cv::Mat &disparityMap, IN const bool isGpu) const override final; + static std::shared_ptr create(const Params& params); + private: +}; +} // namespace cameras +} // namespace slmaster + +#endif // __BINO_THREE_FREQUENCY_HETERODYNE_PATTERN_H_ \ No newline at end of file diff --git a/src/cameras/binocular/binoocularCamer.cpp b/src/cameras/binocular/binoocularCamer.cpp index 9d33818..fca6178 100644 --- a/src/cameras/binocular/binoocularCamer.cpp +++ b/src/cameras/binocular/binoocularCamer.cpp @@ -1,6 +1,5 @@ #include "binoocularCamera.h" -#include "binosSinusCompleGrayCodePattern.h" #include "../tool.h" namespace slmaster { diff --git a/src/cameras/cameras.h b/src/cameras/cameras.h index 55e0545..10d6b7e 100644 --- a/src/cameras/cameras.h +++ b/src/cameras/cameras.h @@ -22,11 +22,13 @@ #include "monocular/monoSinusCompleGrayCodePattern.h" #include "monocular/monoSinusShiftGrayCodePattern.h" #include "monocular/monoInterzoneSinusFourGrayscalePattern.h" + #include "monocular/monoThreeFrequencyHeterodynePattern.h" #include "binocular/binoocularCamera.h" - #include "binocular/binosSinusCompleGrayCodePattern.h" - #include "binocular/binosSinusShiftGrayCodePattern.h" - #include "binocular/binosInterzoneSinusFourGrayscalePattern.h" + #include "binocular/binoSinusCompleGrayCodePattern.h" + #include "binocular/binoSinusShiftGrayCodePattern.h" + #include "binocular/binoInterzoneSinusFourGrayscalePattern.h" + #include "binocular/binoThreeFrequencyHeterodynePattern.h" #include "trinocular/trinocularCamera.h" #include "trinocular/trinocularMultiViewStereoGeometryPattern.h" diff --git a/src/cameras/monocular/monoInterzoneSinusFourGrayscalePattern.cpp b/src/cameras/monocular/monoInterzoneSinusFourGrayscalePattern.cpp index b46564e..7edebd6 100644 --- a/src/cameras/monocular/monoInterzoneSinusFourGrayscalePattern.cpp +++ b/src/cameras/monocular/monoInterzoneSinusFourGrayscalePattern.cpp @@ -110,7 +110,7 @@ bool MonoInterzoneSinusFourGrayscalePattern::decode( pattern->computePhaseMap(patternImages[0], wrappedMap); pattern->computeConfidenceMap(patternImages[0], confidenceMap); pattern->computeFloorMap(patternImages[0], confidenceMap, floorMap); - pattern->unwrapPhaseMap(wrappedMap, confidenceMap, floorMap, unwrappedMap); + pattern->unwrapPhaseMap(wrappedMap, floorMap, confidenceMap, unwrappedMap); reverseCamera(unwrappedMap, params__.PL1_, params__.PR4_, params__.minDepth_, params__.maxDepth_, diff --git a/src/cameras/monocular/monoSinusCompleGrayCodePattern.cpp b/src/cameras/monocular/monoSinusCompleGrayCodePattern.cpp index f5223f9..01c2653 100644 --- a/src/cameras/monocular/monoSinusCompleGrayCodePattern.cpp +++ b/src/cameras/monocular/monoSinusCompleGrayCodePattern.cpp @@ -109,8 +109,7 @@ bool MonoSinusCompleGrayCodePattern::decode( pattern->computeConfidenceMap(patternImages[0], confidenceMap); pattern->computeFloorMap(patternImages[0], confidenceMap, wrappedMap, floorMap); - pattern->unwrapPhaseMap(wrappedMap, floorMap, unwrappedMap, - confidenceMap > params__.confidenceThreshold_); + pattern->unwrapPhaseMap(wrappedMap, floorMap, confidenceMap, unwrappedMap); reverseCamera(unwrappedMap, params__.PL1_, params__.PR4_, params__.minDepth_, params__.maxDepth_, diff --git a/src/cameras/monocular/monoSinusShiftGrayCodePattern.cpp b/src/cameras/monocular/monoSinusShiftGrayCodePattern.cpp index b148401..3d1e67e 100644 --- a/src/cameras/monocular/monoSinusShiftGrayCodePattern.cpp +++ b/src/cameras/monocular/monoSinusShiftGrayCodePattern.cpp @@ -111,8 +111,7 @@ bool MonoSinusShiftGrayCodePattern::decode( pattern->computeConfidenceMap(patternImages[0], confidenceMap); pattern->computeFloorMap(patternImages[0], confidenceMap, wrappedMap, floorMap); - pattern->unwrapPhaseMap(wrappedMap, floorMap, unwrappedMap, - confidenceMap > params__.confidenceThreshold_); + pattern->unwrapPhaseMap(wrappedMap, floorMap, confidenceMap, unwrappedMap); reverseCamera(unwrappedMap, params__.PL1_, params__.PR4_, params__.minDepth_, params__.maxDepth_, diff --git a/src/cameras/monocular/monoThreeFrequencyHeterodynePattern.cpp b/src/cameras/monocular/monoThreeFrequencyHeterodynePattern.cpp new file mode 100644 index 0000000..2acb095 --- /dev/null +++ b/src/cameras/monocular/monoThreeFrequencyHeterodynePattern.cpp @@ -0,0 +1,75 @@ +#include "monoThreeFrequencyHeterodynePattern.h" + +#include "../../algorithm/algorithm.h" + +using namespace cv; +using namespace cv::cuda; +using namespace std; + +namespace slmaster { + +using namespace algorithm; + +namespace cameras { + +static MonoThreeFrequencyHeterodynePattern::Params params__; + +MonoThreeFrequencyHeterodynePattern::MonoThreeFrequencyHeterodynePattern() {} + +std::shared_ptr +MonoThreeFrequencyHeterodynePattern::create(const Params ¶ms) { + params__ = params; + + return std::make_shared(); +} + +bool MonoThreeFrequencyHeterodynePattern::generate(vector &imgs) const { + + ThreeFrequencyHeterodynePattern::Params params; + params.width = params__.width_; + params.height = params__.height_; + params.nbrOfPeriods = params__.cycles_; + params.horizontal = params__.horizontal_; + params.confidenceThreshold = params__.confidenceThreshold_; + params.shiftTime = params__.shiftTime_; + + return ThreeFrequencyHeterodynePattern::create(params)->generate(imgs); +} + +bool MonoThreeFrequencyHeterodynePattern::decode( + const vector> &patternImages, Mat &depthMap, + const bool isGpu) const { + CV_Assert(patternImages.size() >= 1); + +#ifdef OPENCV_WITH_CUDA_MODULE + if (isGpu) { + } +#endif + + ThreeFrequencyHeterodynePattern::Params params; + params.shiftTime = params__.shiftTime_; + params.confidenceThreshold = params__.confidenceThreshold_; + params.height = params__.height_; + params.width = params__.width_; + params.nbrOfPeriods = params__.cycles_; + params.horizontal = params__.horizontal_; + + auto pattern = ThreeFrequencyHeterodynePattern::create(params); + Mat wrappedMap, confidenceMap, floorMap, unwrappedMap; + + pattern->computePhaseMap(patternImages[0], wrappedMap); + pattern->computeConfidenceMap(patternImages[0], confidenceMap); + pattern->computeFloorMap(patternImages[0], confidenceMap, floorMap); + pattern->unwrapPhaseMap(wrappedMap, floorMap, confidenceMap, unwrappedMap); + + reverseCamera(unwrappedMap, params__.PL1_, params__.PR4_, + params__.minDepth_, params__.maxDepth_, + static_cast(params__.horizontal_ ? params__.height_ + : params__.width_) / + params__.cycles_, + depthMap, params__.horizontal_); + + return true; +} +} // namespace cameras +} // namespace slmaster \ No newline at end of file diff --git a/src/cameras/monocular/monoThreeFrequencyHeterodynePattern.h b/src/cameras/monocular/monoThreeFrequencyHeterodynePattern.h new file mode 100644 index 0000000..b3dd8d1 --- /dev/null +++ b/src/cameras/monocular/monoThreeFrequencyHeterodynePattern.h @@ -0,0 +1,52 @@ +/** + * @file monoThreeFrequencyHeterodynePattern.h + * @author Evans Liu (1369215984@qq.com) + * @brief + * @version 0.1 + * @date 2024-04-17 + * + * @copyright Copyright (c) 2024 + * + */ + +#ifndef __MONO_THREE_FREQUENCY_HETERODYNE_PATTERN_H_ +#define __MONO_THREE_FREQUENCY_HETERODYNE_PATTERN_H_ + +#include "../../common.h" +#include "../pattern.h" + +namespace slmaster { +namespace cameras { +// TODO@Evans +// Liu:使用修饰器模式会更好，避免未来方法增多导致的子类爆炸，需要在OpenCV中重新更改接口 +class SLMASTER_API MonoThreeFrequencyHeterodynePattern : public Pattern { + public: + struct SLMASTER_API Params { + Params() + : width_(1920), height_(1080), shiftTime_(4), cycles_(32), + horizontal_(false), confidenceThreshold_(5.f) {} + int width_; + int height_; + int shiftTime_; + int cycles_; + bool horizontal_; + float confidenceThreshold_; + float minDepth_; + float maxDepth_; + Eigen::Matrix4f PL1_; + Eigen::Matrix4f PR4_; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + MonoThreeFrequencyHeterodynePattern(); + virtual bool generate(IN std::vector &imgs) const override final; + virtual bool + decode(IN const std::vector> &patternImages, + OUT cv::Mat &depthMap, IN const bool isGpu) const override final; + static std::shared_ptr create(const Params& params); + private: +}; +} // namespace cameras +} // namespace slmaster + +#endif // __MONO_THREE_FREQUENCY_HETERODYNE_PATTERN_H_ \ No newline at end of file diff --git a/src/cameras/trinocular/trinocularCamera.cpp b/src/cameras/trinocular/trinocularCamera.cpp index 1ed8cec..56f31c3 100644 --- a/src/cameras/trinocular/trinocularCamera.cpp +++ b/src/cameras/trinocular/trinocularCamera.cpp @@ -1,6 +1,6 @@ #include "trinocularCamera.h" -#include "../binocular/binosSinusCompleGrayCodePattern.h" +#include "../binocular/binoSinusCompleGrayCodePattern.h" #include "../tool.h" namespace slmaster { diff --git a/test/testInterzoneSinusFourGrayscalePattern.cpp b/test/testInterzoneSinusFourGrayscalePattern.cpp index 4d22ef0..faf7a09 100644 --- a/test/testInterzoneSinusFourGrayscalePattern.cpp +++ b/test/testInterzoneSinusFourGrayscalePattern.cpp @@ -55,7 +55,7 @@ TEST_F(InterzoneSinusFourGrayscalePatternSuit, testGenerateUnwrap) { pattern->computePhaseMap(imgs, wrappedPhaseMap); pattern->computeConfidenceMap(imgs, confidenceMap); pattern->computeFloorMap(imgs, confidenceMap, floorMap); - pattern->unwrapPhaseMap(wrappedPhaseMap, confidenceMap, floorMap, + pattern->unwrapPhaseMap(wrappedPhaseMap, floorMap, confidenceMap, unwrapPhaseMap); ASSERT_EQ(floorMap.ptr(330)[515], 4); @@ -77,9 +77,9 @@ TEST_F(InterzoneSinusFourGrayscalePatternSuit, testUnwrap) { pattern->computePhaseMap(imgs, wrappedPhaseMap); pattern->computeConfidenceMap(imgs, confidenceMap); pattern->computeFloorMap(imgs, confidenceMap, floorMap); - pattern->unwrapPhaseMap(wrappedPhaseMap, confidenceMap, floorMap, + pattern->unwrapPhaseMap(wrappedPhaseMap, floorMap, confidenceMap, unwrapPhaseMap); - + ASSERT_EQ(floorMap.ptr(486)[660], 8); ASSERT_LE(abs(unwrapPhaseMap.ptr(384)[656] - 51.8f), 0.1f); } \ No newline at end of file diff --git a/test/testShiftGrayCodePattern.cpp b/test/testShiftGrayCodePattern.cpp index 7056959..d9f3864 100644 --- a/test/testShiftGrayCodePattern.cpp +++ b/test/testShiftGrayCodePattern.cpp @@ -54,7 +54,7 @@ TEST_F(ShiftGrayCodePatternSuit, testGenerateUnwrap) { pattern->computePhaseMap(imgs, wrapPhaseMap); pattern->computeConfidenceMap(imgs, confidenceMap); pattern->computeFloorMap(imgs, confidenceMap, wrapPhaseMap, floorMap); - pattern->unwrapPhaseMap(wrapPhaseMap, floorMap, unwrapPhaseMap); + pattern->unwrapPhaseMap(wrapPhaseMap, floorMap, confidenceMap, unwrapPhaseMap); ASSERT_EQ(floorMap.ptr(444)[780], 12); ASSERT_EQ(floorMap.ptr(444)[781], 13); @@ -75,7 +75,7 @@ TEST_F(ShiftGrayCodePatternSuit, testUnwrap) { pattern->computePhaseMap(imgs, wrapPhaseMap); pattern->computeConfidenceMap(imgs, confidenceMap); pattern->computeFloorMap(imgs, confidenceMap, wrapPhaseMap, floorMap); - pattern->unwrapPhaseMap(wrapPhaseMap, floorMap, unwrapPhaseMap); + pattern->unwrapPhaseMap(wrapPhaseMap, floorMap, confidenceMap, unwrapPhaseMap); ASSERT_EQ(floorMap.ptr(453)[700], 17); ASSERT_LE(abs(unwrapPhaseMap.ptr(460)[653] - 103.75), 0.1f); diff --git a/test/testThreeFrequencyHeterodynePattern.cpp b/test/testThreeFrequencyHeterodynePattern.cpp index ac3e687..6bd66b0 100644 --- a/test/testThreeFrequencyHeterodynePattern.cpp +++ b/test/testThreeFrequencyHeterodynePattern.cpp @@ -12,12 +12,10 @@ const string imgsPath = "../../data/threeFrequencyHeterodyne/"; class ThreeFrequencyHeterodynePatternSuit : public testing::Test { public: void SetUp() override { - /* - for (int i = 0; i < 5; ++i) { + for (int i = 0; i < 12; ++i) { Mat temp = imread(imgsPath + to_string(i) + ".bmp", 0); imgs.emplace_back(temp); } - */ } vector imgs; @@ -38,7 +36,7 @@ TEST_F(ThreeFrequencyHeterodynePatternSuit, testGenerate) { ASSERT_EQ(imgs[0].ptr(420)[730], 191); ASSERT_EQ(imgs[3].ptr(420)[730], 17); - ASSERT_EQ(imgs[6].ptr(420)[730], 176); + ASSERT_EQ(imgs[6].ptr(420)[730], 248); } TEST_F(ThreeFrequencyHeterodynePatternSuit, testGenerateUnwrap) { @@ -47,7 +45,7 @@ TEST_F(ThreeFrequencyHeterodynePatternSuit, testGenerateUnwrap) { params.height = 1080; params.width = 1920; params.horizontal = false; - params.nbrOfPeriods = 64; + params.nbrOfPeriods = 70; auto pattern = ThreeFrequencyHeterodynePattern::create(params); @@ -59,11 +57,11 @@ TEST_F(ThreeFrequencyHeterodynePatternSuit, testGenerateUnwrap) { pattern->computePhaseMap(imgs, wrappedPhaseMaps); pattern->computeConfidenceMap(imgs, confidenceMap); pattern->computeFloorMap(wrappedPhaseMaps, confidenceMap, floorMap); - pattern->unwrapPhaseMap(wrappedPhaseMaps, confidenceMap, floorMap, + pattern->unwrapPhaseMap(wrappedPhaseMaps[0], floorMap, confidenceMap, unwrapPhaseMap); - ASSERT_EQ(floorMap.ptr(420)[730], 191); - ASSERT_LE(abs(unwrapPhaseMap.ptr(410)[685] - 35.866), 0.1f); + ASSERT_EQ(floorMap.ptr(346)[866], 31); + ASSERT_LE(abs(unwrapPhaseMap.ptr(500)[1050] - 240.53), 0.1f); } TEST_F(ThreeFrequencyHeterodynePatternSuit, testUnwrap) { @@ -81,10 +79,10 @@ TEST_F(ThreeFrequencyHeterodynePatternSuit, testUnwrap) { Mat floorMap, confidenceMap, unwrapPhaseMap; pattern->computePhaseMap(imgs, wrappedPhaseMaps); pattern->computeConfidenceMap(imgs, confidenceMap); - pattern->computeFloorMap(imgs, confidenceMap, floorMap); - pattern->unwrapPhaseMap(wrappedPhaseMaps, confidenceMap, floorMap, + pattern->computeFloorMap(wrappedPhaseMaps, confidenceMap, floorMap); + pattern->unwrapPhaseMap(wrappedPhaseMaps[0], floorMap, confidenceMap, unwrapPhaseMap); - - ASSERT_EQ(floorMap.ptr(486)[660], 8); - ASSERT_LE(abs(unwrapPhaseMap.ptr(384)[656] - 51.8f), 0.1f); + + ASSERT_EQ(floorMap.ptr(480)[590], 30); + ASSERT_LE(abs(unwrapPhaseMap.ptr(470)[590] - 191.95f), 0.1f); } \ No newline at end of file