From 35d91c25b92d51de074236c80fb1dcfc1b772efa Mon Sep 17 00:00:00 2001 From: TinyO Date: Sat, 17 Aug 2019 18:52:31 +0800 Subject: [PATCH] fix(calib): covert params change. --- include/mynteye/types.h | 1 + src/mynteye/api/processor/disparity_processor.cc | 9 ++++++++- src/mynteye/api/processor/disparity_processor.h | 2 ++ src/mynteye/api/processor/rectify_processor.cc | 8 +++++++- src/mynteye/api/processor/rectify_processor.h | 1 + src/mynteye/api/synthetic.cc | 12 +++++++++--- 6 files changed, 28 insertions(+), 5 deletions(-) diff --git a/include/mynteye/types.h b/include/mynteye/types.h index e22b838..7d5a3a5 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -775,6 +775,7 @@ struct CameraROSMsgInfoPair { struct CameraROSMsgInfo left; struct CameraROSMsgInfo right; double T_mul_f = -1.f; + double cx1_minus_cx2 = 0.f; double R[9] = {0}; double P[12] = {0}; }; diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index f946e75..66d4def 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -27,8 +27,14 @@ MYNTEYE_BEGIN_NAMESPACE const char DisparityProcessor::NAME[] = "DisparityProcessor"; DisparityProcessor::DisparityProcessor(DisparityComputingMethod type, + std::shared_ptr calib_infos, std::int32_t proc_period) : Processor(std::move(proc_period)), type_(type) { + if (calib_infos) { + cx1_minus_cx2_ = calib_infos->cx1_minus_cx2; + } else { + cx1_minus_cx2_ = 1.f; + } VLOG(2) << __func__ << ": proc_period=" << proc_period; int sgbmWinSize = 3; int numberOfDisparities = 64; @@ -156,6 +162,7 @@ bool DisparityProcessor::OnProcess( } else if (type_ == DisparityComputingMethod::BM) { // LOG(ERROR) << "not supported in opencv 2.x"; (*sgbm_matcher)(input->first, input->second, disparity); + disparity.convertTo(output->value, CV_32F, 1./16, 1); // cv::Mat tmp1, tmp2; // cv::cvtColor(input->first, tmp1, CV_RGB2GRAY); // cv::cvtColor(input->second, tmp2, CV_RGB2GRAY); @@ -188,7 +195,7 @@ bool DisparityProcessor::OnProcess( sgbm_matcher->compute(input->first, input->second, disparity); } #endif - disparity.convertTo(output->value, CV_32F, 1./16, 1); + disparity.convertTo(output->value, CV_32F, 1./16, cx1_minus_cx2_); output->id = input->first_id; output->data = input->first_data; return true; diff --git a/src/mynteye/api/processor/disparity_processor.h b/src/mynteye/api/processor/disparity_processor.h index 5c190b7..3f3a6d9 100644 --- a/src/mynteye/api/processor/disparity_processor.h +++ b/src/mynteye/api/processor/disparity_processor.h @@ -32,6 +32,7 @@ class DisparityProcessor : public Processor { static const char NAME[]; explicit DisparityProcessor(DisparityComputingMethod type, + std::shared_ptr calib_infos, std::int32_t proc_period = 0); virtual ~DisparityProcessor(); @@ -54,6 +55,7 @@ class DisparityProcessor : public Processor { cv::Ptr sgbm_matcher; cv::Ptr bm_matcher; DisparityComputingMethod type_; + double cx1_minus_cx2_; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 132da35..9a136de 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -42,6 +42,7 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo, const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* matR, const CvMat* matT, CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f, + double *cx1_min_cx2, int flags, double alpha, CvSize newImgSize) { // std::cout << _alpha << std::endl; alpha = _alpha; @@ -218,6 +219,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo, cvmSet(_P2, 0, 2, cx2); cvmSet(_P2, 1, 2, cy2); cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3)); + + *cx1_min_cx2 = -(cx1 - cx2); } } @@ -299,8 +302,10 @@ std::shared_ptr RectifyProcessor::stereoRectify( CvMat c_K1 = K1, c_K2 = K2, c_D1 = D1, c_D2 = D2; CvMat c_R1 = R1, c_R2 = R2, c_P1 = P1, c_P2 = P2; double T_mul_f; + double cx1_min_cx2; stereoRectify(leftOdo, rightOdo, &c_K1, &c_K2, &c_D1, &c_D2, - image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f); + image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f, + &cx1_min_cx2); VLOG(2) << "K1: " << K1 << std::endl; VLOG(2) << "D1: " << D1 << std::endl; @@ -334,6 +339,7 @@ std::shared_ptr RectifyProcessor::stereoRectify( info_pair.left = calib_mat_data_left; info_pair.right = calib_mat_data_right; info_pair.T_mul_f = T_mul_f; + info_pair.cx1_minus_cx2 = cx1_min_cx2; for (std::size_t i = 0; i< 3 * 4; i++) { info_pair.P[i] = calib_mat_data_left.P[i]; } diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 900877c..14fd3f3 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -87,6 +87,7 @@ class RectifyProcessor : public Processor { const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* matR, const CvMat* matT, CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f, + double *cx1_min_cx2, int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, CvSize newImgSize = cv::Size()); diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index d0782b0..8df030d 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -313,10 +313,8 @@ void Synthetic::InitProcessors() { std::shared_ptr rectify_processor = nullptr; std::shared_ptr points_processor = nullptr; std::shared_ptr depth_processor = nullptr; + std::shared_ptr disparity_processor = nullptr; - auto &&disparity_processor = - std::make_shared(DisparityComputingMethod::BM, - DISPARITY_PROC_PERIOD); auto &&disparitynormalized_processor = std::make_shared( DISPARITY_NORM_PROC_PERIOD); @@ -333,6 +331,10 @@ void Synthetic::InitProcessors() { 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); root_processor->AddChild(rectify_processor); rectify_processor->AddChild(disparity_processor); @@ -352,6 +354,10 @@ void Synthetic::InitProcessors() { depth_processor = std::make_shared( rectify_processor_imp -> getCameraROSMsgInfoPair(), DEPTH_PROC_PERIOD); + disparity_processor = + std::make_shared(DisparityComputingMethod::BM, + rectify_processor_imp -> getCameraROSMsgInfoPair(), + DISPARITY_PROC_PERIOD); root_processor->AddChild(rectify_processor); rectify_processor->AddChild(disparity_processor);