From 1adb715b5160e4964ee9c094d75553ea5ad34ad3 Mon Sep 17 00:00:00 2001 From: TinyO Date: Mon, 2 Sep 2019 18:03:30 +0800 Subject: [PATCH] fix(*): use auto min_dis/max_dis instead of static one. --- src/mynteye/api/processor/depth_processor.cc | 10 +++++++-- src/mynteye/api/processor/depth_processor.h | 5 +++++ .../api/processor/disparity_processor.cc | 17 ++++++++++----- .../api/processor/disparity_processor.h | 21 ++++++++++--------- src/mynteye/api/synthetic.cc | 13 +++++++----- 5 files changed, 44 insertions(+), 22 deletions(-) diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index 8754540..e1ca22d 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -26,9 +26,13 @@ int DISPARITY_MAX = 64; DepthProcessor::DepthProcessor( std::shared_ptr calib_infos, + std::shared_ptr min_disp, + std::shared_ptr max_disp, std::int32_t proc_period) : Processor(std::move(proc_period)), - calib_infos_(calib_infos) { + calib_infos_(calib_infos), + min_disp_(min_disp), + max_disp_(max_disp) { VLOG(2) << __func__; } @@ -54,11 +58,13 @@ bool DepthProcessor::OnProcess( int cols = input->value.cols; // 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++) { for (int j = 0; j < cols; j++) { float disparity_value = input->value.at(i, j); - if (disparity_value < DISPARITY_MAX && disparity_value > DISPARITY_MIN) { + if (disparity_value < (max_disp_ ? *max_disp_ : DISPARITY_MAX) && + disparity_value > (min_disp_ ? *min_disp_ : DISPARITY_MIN)) { float depth = calib_infos_->T_mul_f / disparity_value; depth_mat.at(i, j) = depth; } diff --git a/src/mynteye/api/processor/depth_processor.h b/src/mynteye/api/processor/depth_processor.h index c9b152f..f247386 100644 --- a/src/mynteye/api/processor/depth_processor.h +++ b/src/mynteye/api/processor/depth_processor.h @@ -16,6 +16,7 @@ #pragma once #include +#include #include "mynteye/api/processor.h" #include "mynteye/api/processor/rectify_processor.h" @@ -29,6 +30,8 @@ class DepthProcessor : public Processor { explicit DepthProcessor( std::shared_ptr calib_infos, + std::shared_ptr min_disp = nullptr, + std::shared_ptr max_disp = nullptr, std::int32_t proc_period = 0); virtual ~DepthProcessor(); @@ -44,6 +47,8 @@ class DepthProcessor : public Processor { std::shared_ptr const parent) override; private: std::shared_ptr calib_infos_; + std::shared_ptr min_disp_; + std::shared_ptr max_disp_; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index 3436cd9..002040d 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -81,9 +81,10 @@ DisparityProcessor::DisparityProcessor(DisparityComputingMethod type, sgbm_matcher->setSpeckleWindowSize(100); sgbm_matcher->setSpeckleRange(32); sgbm_matcher->setDisp12MaxDiff(1); - disparity_min_sgbm = sgbm_matcher->getMinDisparity(); - disparity_max_sgbm = sgbm_matcher->getNumDisparities(); - + disparity_min_sgbm_ptr = + std::make_shared(sgbm_matcher->getMinDisparity()); + disparity_max_sgbm_ptr = + std::make_shared(sgbm_matcher->getNumDisparities()); bm_matcher = cv::StereoBM::create(0, 3); bm_matcher->setPreFilterSize(9); bm_matcher->setPreFilterCap(31); @@ -95,8 +96,10 @@ DisparityProcessor::DisparityProcessor(DisparityComputingMethod type, bm_matcher->setSpeckleWindowSize(100); bm_matcher->setSpeckleRange(4); bm_matcher->setPreFilterType(cv::StereoBM::PREFILTER_XSOBEL); - disparity_min_bm = bm_matcher->getMinDisparity(); - disparity_max_bm = bm_matcher->getNumDisparities(); + disparity_min_bm_ptr = + std::make_shared(bm_matcher->getMinDisparity()); + disparity_max_bm_ptr = + std::make_shared(bm_matcher->getNumDisparities()); #endif NotifyComputingTypeChanged(type_); } @@ -125,11 +128,15 @@ bool DisparityProcessor::ConfigFromFile(const std::string& config_file) { cv::FileNode node_sgbm = fsSettings["SGBM"]; if (node_sgbm.type() == cv::FileNode::MAP) { sgbm_matcher->read(node_sgbm); + *disparity_min_sgbm_ptr = sgbm_matcher->getMinDisparity(); + *disparity_max_sgbm_ptr = sgbm_matcher->getNumDisparities(); } cv::FileNode node_bm = fsSettings["BM"]; if (node_bm.type() == cv::FileNode::MAP) { bm_matcher->read(node_bm); + *disparity_min_bm_ptr = bm_matcher->getMinDisparity(); + *disparity_max_bm_ptr = bm_matcher->getNumDisparities(); } return true; #else diff --git a/src/mynteye/api/processor/disparity_processor.h b/src/mynteye/api/processor/disparity_processor.h index 4075d9f..b66b54b 100644 --- a/src/mynteye/api/processor/disparity_processor.h +++ b/src/mynteye/api/processor/disparity_processor.h @@ -16,6 +16,7 @@ #pragma once #include +#include #include "mynteye/api/processor.h" #include "mynteye/types.h" @@ -41,18 +42,18 @@ class DisparityProcessor : public Processor { const DisparityComputingMethod &MethodType); void NotifyComputingTypeChanged(const DisparityComputingMethod &MethodType); bool ConfigFromFile(const std::string& config); - double GetMinDisparity() { + std::shared_ptr GetMinDisparity() { if (type_ == DisparityComputingMethod::BM) { - return disparity_min_bm; + return disparity_min_bm_ptr; } else { - return disparity_min_sgbm; + return disparity_min_sgbm_ptr; } } - double GetMaxDisparity() { + std::shared_ptr GetMaxDisparity() { if (type_ == DisparityComputingMethod::BM) { - return disparity_max_bm; + return disparity_max_bm_ptr; } else { - return disparity_max_sgbm; + return disparity_max_sgbm_ptr; } } @@ -70,10 +71,10 @@ class DisparityProcessor : public Processor { cv::Ptr bm_matcher; DisparityComputingMethod type_; double cx1_minus_cx2_; - double disparity_min_bm; - double disparity_max_bm; - double disparity_min_sgbm; - double disparity_max_sgbm; + std::shared_ptr disparity_min_bm_ptr; + std::shared_ptr disparity_max_bm_ptr; + std::shared_ptr disparity_min_sgbm_ptr; + std::shared_ptr disparity_max_sgbm_ptr; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 23ea307..168be88 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -330,11 +330,11 @@ void Synthetic::InitProcessors() { rectify_processor = rectify_processor_ocv; points_processor = std::make_shared( rectify_processor_ocv->Q, POINTS_PROC_PERIOD); - depth_processor = std::make_shared(DEPTH_PROC_PERIOD); disparity_processor = std::make_shared(DisparityComputingMethod::BM, nullptr, DISPARITY_PROC_PERIOD); + depth_processor = std::make_shared(DEPTH_PROC_PERIOD); root_processor->AddChild(rectify_processor); rectify_processor->AddChild(disparity_processor); @@ -351,13 +351,16 @@ void Synthetic::InitProcessors() { points_processor = std::make_shared( rectify_processor_imp -> getCameraROSMsgInfoPair(), POINTS_PROC_PERIOD); - depth_processor = std::make_shared( - rectify_processor_imp -> getCameraROSMsgInfoPair(), - DEPTH_PROC_PERIOD); - disparity_processor = + auto disparity_processor_imp = std::make_shared(DisparityComputingMethod::BM, rectify_processor_imp -> getCameraROSMsgInfoPair(), DISPARITY_PROC_PERIOD); + depth_processor = std::make_shared( + rectify_processor_imp -> getCameraROSMsgInfoPair(), + disparity_processor_imp->GetMinDisparity(), + disparity_processor_imp->GetMaxDisparity(), + DEPTH_PROC_PERIOD); + disparity_processor = disparity_processor_imp; root_processor->AddChild(rectify_processor); rectify_processor->AddChild(disparity_processor);