From 7b8ed810188291c41da31249c7950272f6502893 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Tue, 15 Jan 2019 10:07:58 +0800 Subject: [PATCH] fix(bm matcher): add complie switch --- cmake/Option.cmake | 1 + include/mynteye/mynteye.h.in | 1 + src/mynteye/api/processor/depth_processor.cc | 2 +- .../api/processor/disparity_processor.cc | 45 ++++++++++++++++++- .../api/processor/disparity_processor.h | 1 - src/mynteye/api/synthetic.cc | 2 +- 6 files changed, 47 insertions(+), 5 deletions(-) diff --git a/cmake/Option.cmake b/cmake/Option.cmake index 7380672..00fc825 100644 --- a/cmake/Option.cmake +++ b/cmake/Option.cmake @@ -24,6 +24,7 @@ option(WITH_API "Build with API layer, need OpenCV" ON) option(WITH_DEVICE_INFO_REQUIRED "Build with device info required" ON) option(WITH_CAM_MODELS "Build with more camera models, WITH_API must be ON" OFF) +option(WITH_BM_SOBEL_FILTER "Build with bm and sobel filter, need OpenCV contronb" OFF) # 3rdparty components diff --git a/include/mynteye/mynteye.h.in b/include/mynteye/mynteye.h.in index bd2fafb..7872768 100644 --- a/include/mynteye/mynteye.h.in +++ b/include/mynteye/mynteye.h.in @@ -71,6 +71,7 @@ MYNTEYE_END_NAMESPACE #cmakedefine WITH_API #cmakedefine WITH_DEVICE_INFO_REQUIRED #cmakedefine WITH_CAM_MODELS +#cmakedefine WITH_BM_SOBEL_FILTER #cmakedefine WITH_OPENCV #cmakedefine WITH_OPENCV2 diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index ae8f5e5..9441132 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -51,7 +51,7 @@ bool DepthProcessor::OnProcess( ObjMat *output = Object::Cast(out); int rows = input->value.rows; int cols = input->value.cols; - std::cout << calib_infos_->T_mul_f << std::endl; + // std::cout << calib_infos_->T_mul_f << std::endl; // 0.0793434 cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_16U); for (int i = 0; i < rows; i++) { diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index dff775d..4454eab 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -17,6 +17,9 @@ #include #include +#ifdef WITH_BM_SOBEL_FILTER +#include +#endif #include "mynteye/logger.h" @@ -62,6 +65,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type, sgbm_matcher->setSpeckleRange(32); sgbm_matcher->setDisp12MaxDiff(1); #endif +#ifdef WITH_BM_SOBEL_FILTER } else if (type_ == DisparityProcessorType::BM) { #ifdef WITH_OPENCV2 int bmWinSize = 3; @@ -90,9 +94,42 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type, bm_matcher->setSpeckleWindowSize(100); bm_matcher->setSpeckleRange(4); bm_matcher->setPreFilterType(cv::StereoBM::PREFILTER_XSOBEL); +#endif #endif } else { - LOG(ERROR) << "no enum DisparityProcessorType" << static_cast(type); + LOG(ERROR) << "no enum DisparityProcessorType,use default sgbm"; + int sgbmWinSize = 3; + int numberOfDisparities = 64; + +#ifdef WITH_OPENCV2 + // StereoSGBM + // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?#stereosgbm + sgbm_matcher = cv::Ptr( + new cv::StereoSGBM( + 0, // minDisparity + numberOfDisparities, // numDisparities + sgbmWinSize, // SADWindowSize + 8 * sgbmWinSize * sgbmWinSize, // P1 + 32 * sgbmWinSize * sgbmWinSize, // P2 + 1, // disp12MaxDiff + 63, // preFilterCap + 10, // uniquenessRatio + 100, // speckleWindowSize + 32, // speckleRange + false)); // fullDP +#else + sgbm_matcher = cv::StereoSGBM::create(0, 16, 3); + sgbm_matcher->setPreFilterCap(63); + sgbm_matcher->setBlockSize(sgbmWinSize); + sgbm_matcher->setP1(8 * sgbmWinSize * sgbmWinSize); + sgbm_matcher->setP2(32 * sgbmWinSize * sgbmWinSize); + sgbm_matcher->setMinDisparity(0); + sgbm_matcher->setNumDisparities(numberOfDisparities); + sgbm_matcher->setUniquenessRatio(10); + sgbm_matcher->setSpeckleWindowSize(100); + sgbm_matcher->setSpeckleRange(32); + sgbm_matcher->setDisp12MaxDiff(1); +#endif } } @@ -138,12 +175,16 @@ bool DisparityProcessor::OnProcess( // whereas other algorithms output 32-bit floating-point disparity map. if (type_ == DisparityProcessorType::SGBM) { sgbm_matcher->compute(input->first, input->second, disparity); +#ifdef WITH_BM_SOBEL_FILTER } else if (type_ == DisparityProcessorType::BM) { - CvSize size = input->first.size(); cv::Mat tmp1, tmp2; cv::cvtColor(input->first, tmp1, CV_RGB2GRAY); cv::cvtColor(input->second, tmp2, CV_RGB2GRAY); bm_matcher->compute(tmp1, tmp2, disparity); +#endif + } else { + // default + sgbm_matcher->compute(input->first, input->second, disparity); } #endif disparity.convertTo(output->value, CV_32F, 1./16, 1); diff --git a/src/mynteye/api/processor/disparity_processor.h b/src/mynteye/api/processor/disparity_processor.h index f7fd952..3595185 100644 --- a/src/mynteye/api/processor/disparity_processor.h +++ b/src/mynteye/api/processor/disparity_processor.h @@ -16,7 +16,6 @@ #pragma once #include -#include #include "mynteye/api/processor.h" namespace cv { diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index ccd3c11..ca695fa 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -613,7 +613,7 @@ void Synthetic::InitProcessors() { rectify_processor = rectify_processor_ocv; } auto &&disparity_processor = - std::make_shared(DisparityProcessorType::BM, + std::make_shared(DisparityProcessorType::SGBM, DISPARITY_PROC_PERIOD); auto &&disparitynormalized_processor = std::make_shared(