diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index bf9747d..10ce715 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -48,14 +48,14 @@ 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; + // 0.0793434 // TODO(MYNTEYE): Put the corresponding parameters(T,f) - float T = 0.08; - float f = 0.01; cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { float disparity_value = input->value.at(i, j); - float depth = T * f / disparity_value; + float depth = calib_infos_->T_mul_f * 1000.0 / disparity_value ; depth_mat.at(i, j) = depth; } } diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 3e1e6c9..e41c3f8 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -75,12 +75,11 @@ Object *PointsProcessor::OnCreateOutput() { bool PointsProcessor::OnProcess( Object *const in, Object *const out, Processor *const parent) { MYNTEYE_UNUSED(parent) - std::cout << calib_infos_->left.height << std::endl; - float fx = 3.6797709792391299e+02; - float fy = 3.6808712539453859e+02; - float cx = 3.7414963027144353e+02; - float cy = 2.3125000326472903e+02; + float fx = calib_infos_->left.K[0]; + float fy = calib_infos_->left.K[4]; + float cx = calib_infos_->left.K[2]; + float cy = calib_infos_->left.K[5]; // Use correct principal point from calibration float center_x = cx; diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 5e2b862..b246005 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -41,7 +41,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo, camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* matR, const CvMat* matT, - CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, + CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f, int flags, double alpha, CvSize newImgSize) { double _om[3], _t[3] = {0}, _uu[3]={0, 0, 0}, _r_r[3][3], _pp[3][4]; double _ww[3], _wr[3][3], _z[3] = {0, 0, 0}, _ri[3][3], _w3[3]; @@ -67,7 +67,6 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo, cvConvertScale(&om, &om, -0.5); // get average rotation cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging cvMatMul(&r_r, matT, &t); - int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; // if idx == 0 @@ -168,6 +167,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo, _pp[0][2] = cc_new[1].x; _pp[1][2] = cc_new[1].y; _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length + *T_mul_f = 0. - _t[idx]; cvConvert(&pp, _P2); alpha = MIN(alpha, 1.); @@ -277,8 +277,9 @@ std::shared_ptr RectifyProcessor::stereoRectify( CvMat c_R = cv_R, c_t = cv_t; 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; 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); + image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f); // std::cout << "K1: " << K1 << std::endl; // std::cout << "D1: " << D1 << std::endl; @@ -308,7 +309,7 @@ std::shared_ptr RectifyProcessor::stereoRectify( } } struct camera_calib_info_pair res = - {calib_mat_data_left, calib_mat_data_right}; + {calib_mat_data_left, calib_mat_data_right, T_mul_f}; return std::make_shared(res); } diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 782eb6d..046b186 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -53,6 +53,7 @@ struct camera_calib_info { struct camera_calib_info_pair { struct camera_calib_info left; struct camera_calib_info right; + double T_mul_f; }; class Device; @@ -94,7 +95,7 @@ class RectifyProcessor : public Processor { camodocal::CameraPtr rightOdo, const CvMat* K1, const CvMat* K2, const CvMat* D1, const CvMat* D2, CvSize imageSize, const CvMat* matR, const CvMat* matT, - CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, + CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f, int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1, CvSize newImgSize = cv::Size());