Add InitRectifyMap() and change distortionCoeff order

[platform/core/multimedia/dfs-opencv.git] / src / dfs_opencv.cpp

diff --git a/src/dfs_opencv.cpp b/src/dfs_opencv.cpp
index ccde94e..c3299fb 100644 (file)
--- 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;