From 8281820e1e236fa1cc0230d4a76f9cf9dec5eda2 Mon Sep 17 00:00:00 2001 From: Tae-Young Chung Date: Tue, 19 Oct 2021 10:04:08 +0900 Subject: [PATCH] Add InitRectifyMap() and change distortionCoeff order mapping table, which is used to rectify distorted images, is pre-computed in InitRectifyMap(). Before actual depth computation by compute() function of opencv, rectification with mapping table is done by remap() function of opencv. The order of distortion coefficients is k1, k2, p1, p2, and k3, not k1, k2, k3, p1, and p2. Signed-off-by: Tae-Young Chung --- res/stereoCalibZed.yaml | 4 +-- src/dfs_opencv.cpp | 74 ++++++++++++++++++++++++++++++++++++++++++++++-- src/dfs_opencv_private.h | 6 ++++ 3 files changed, 80 insertions(+), 4 deletions(-) diff --git a/res/stereoCalibZed.yaml b/res/stereoCalibZed.yaml index 615f2bd..7bcadad 100644 --- a/res/stereoCalibZed.yaml +++ b/res/stereoCalibZed.yaml @@ -8,7 +8,7 @@ LEFT_CAM_DISTCOEFFS: !!opencv-matrix rows: 5 cols: 1 dt: d - data: [ -0.0397725, 0.00826312, -0.00435501, -0.000558719, 0.000138723 ] + data: [ -0.0397725, 0.00826312, -0.000558719, 0.000138723, -0.00435501] RIGHT_CAM_VGA: !!opencv-matrix rows: 3 cols: 3 @@ -18,7 +18,7 @@ RIGHT_CAM_DISTCOEFFS: !!opencv-matrix rows: 5 cols: 1 dt: d - data: [ -0.0416412, 0.00967152, -0.00473998, 0.000477674, 0.00033336 ] + data: [ -0.0416412, 0.00967152, 0.000477674, 0.00033336, -0.00473998 ] STEREO_TRANSLATION: !!opencv-matrix rows: 3 cols: 1 diff --git a/src/dfs_opencv.cpp b/src/dfs_opencv.cpp index ccde94e..c3299fb 100644 --- a/src/dfs_opencv.cpp +++ b/src/dfs_opencv.cpp @@ -36,7 +36,8 @@ namespace DfsAdaptationImpl mCalibFilePath(DFS_CALIB_FILE_PATH), mStereoParam(), mIsStereoCalibrated(false), - mUpdateStereoCalibration(false) + mUpdateStereoCalibration(false), + mImageSize(640, 480) { LOGI("ENTER"); mCalibFilePath += std::string(DEFAULT_STEREO_CALIB_FILE_NAME); @@ -80,6 +81,70 @@ namespace DfsAdaptationImpl LOGI("LEAVE"); } + void DfsOCV::InitRectifyMap() + { + LOGI("ENTER"); + + try { + cv::Mat newBaseCameraIntrinsic = cv::getOptimalNewCameraMatrix( + mStereoParam.baceCamera.intrinsic, + mStereoParam.baceCamera.distCoeffs, + mImageSize, + 1.0); + + cv::Mat newExtraCameraIntrinsic = cv::getOptimalNewCameraMatrix( + mStereoParam.extraCamera.intrinsic, + mStereoParam.extraCamera.distCoeffs, + mImageSize, + 1.0); + + cv::Mat extraCameraRotation; + cv::Rodrigues(mStereoParam.rotation, extraCameraRotation); + + cv::Mat baseRotation, extraRotation; // 3x3 + cv::Mat baseProjection, extraProjection; // 3x4 + cv::Mat disp2Depth; // 4x4 + + cv::stereoRectify(mStereoParam.baceCamera.intrinsic, + mStereoParam.baceCamera.distCoeffs, + mStereoParam.extraCamera.intrinsic, + mStereoParam.extraCamera.distCoeffs, + mImageSize, + extraCameraRotation, + mStereoParam.translation, + baseRotation, + extraRotation, + baseProjection, + extraProjection, + disp2Depth); + + cv::initUndistortRectifyMap(mStereoParam.baceCamera.intrinsic, + mStereoParam.baceCamera.distCoeffs, + baseRotation, + newBaseCameraIntrinsic, + mImageSize, + CV_16SC2, + mBaseReMap[0], + mBaseReMap[1]); + + cv::initUndistortRectifyMap(mStereoParam.extraCamera.intrinsic, + mStereoParam.extraCamera.distCoeffs, + extraRotation, + newExtraCameraIntrinsic, + mImageSize, + CV_16SC2, + mExtraReMap[0], + mExtraReMap[1]); + + } catch (const std::exception& e) { + LOGE("Failed to InitRectifyMap"); + throw e; + } + + + LOGI("LEAVE"); + + } void DfsOCV::Initialize(DfsParameter& param) { LOGI("ENTER"); @@ -92,6 +157,7 @@ namespace DfsAdaptationImpl try { this->InitializeStereoCalibration(); + this->InitRectifyMap(); } catch (const std::exception& e) { throw e; } @@ -153,7 +219,11 @@ namespace DfsAdaptationImpl throw std::runtime_error("base and extra should be the type"); } - mDfsOcv->compute(baseMat, extraMat, mDispMat); + cv::Mat rBaseMat, rExtraMat; + cv::remap(baseMat, rBaseMat, mBaseReMap[0], mBaseReMap[1], cv::INTER_LINEAR); + cv::remap(extraMat, rExtraMat, mExtraReMap[0], mExtraReMap[1], cv::INTER_LINEAR); + + mDfsOcv->compute(rBaseMat, rExtraMat, mDispMat); mDepthData.data = mDispMat.data; mDepthData.type = DFS_DATA_TYPE_UINT8; diff --git a/src/dfs_opencv_private.h b/src/dfs_opencv_private.h index e52d221..7cce5d5 100644 --- a/src/dfs_opencv_private.h +++ b/src/dfs_opencv_private.h @@ -86,9 +86,15 @@ namespace DfsAdaptationImpl bool mIsStereoCalibrated; bool mUpdateStereoCalibration; + cv::Size mImageSize; + cv::Mat mBaseReMap[2]; + cv::Mat mExtraReMap[2]; + void SetParameters(); int ConvertDfsDataTypeToCV(int type); void InitializeStereoCalibration(); + void InitRectifyMap(); + public: DfsOCV(); ~DfsOCV(); -- 2.7.4