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 <ty83.chung@samsung.com>

diff --git a/res/stereoCalibZed.yaml b/res/stereoCalibZed.yaml
index 615f2bd..7bcadad 100644 (file)
--- 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 (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;

diff --git a/src/dfs_opencv_private.h b/src/dfs_opencv_private.h
index e52d221..7cce5d5 100644 (file)
--- 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();