Change output type from 32float to unsigned 16bit integer

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

diff --git a/src/dfs_opencv.cpp b/src/dfs_opencv.cpp
index b6b2faf..d72f0b2 100644 (file)
--- a/src/dfs_opencv.cpp
+++ b/src/dfs_opencv.cpp
@@ -36,6 +36,7 @@ namespace DfsAdaptationImpl
                mP2(96*1),
                mPreFilterCap(63),
                mDispMat(),
+               mDepthMat(),
                mDepthData(),
                mStereoParam(),
                mIsStereoCalibrated(false),
@@ -83,6 +84,15 @@ namespace DfsAdaptationImpl
                        fs["STEREO_TRANSLATION"] >> mStereoParam.translation;
                        fs["STEREO_ROTATION"] >> mStereoParam.rotation;
 
+                       if (!fs["STEREO_DOFFSET"].empty()) {
+                               mStereoParam.dispOffset = static_cast<float>(fs["STEREO_DOFFSET"]);
+                               LOGI("%f", mStereoParam.dispOffset);
+                       }
+
+                       mDisp2DepthMat = cv::Mat(mImageSize, CV_32F, mStereoParam.baceCamera.intrinsic.at<double>(0,0) *
+                                                                                                                mStereoParam.translation.at<double>(0));
+                       mDispOffsetMat = cv::Mat(mImageSize, CV_32F, mStereoParam.dispOffset);
+
                } catch (const std::exception& e) {
                        LOGE("Failed to read calibration data %s", e.what());
                        throw std::ios_base::failure("calibration");
@@ -449,11 +459,18 @@ namespace DfsAdaptationImpl
                        }
                }
 
-               mDepthData.data = mDispMat.ptr<float>();
-               mDepthData.type = DFS_DATA_TYPE_FLOAT32C1;
-               mDepthData.width = mDispMat.cols;
-               mDepthData.height = mDispMat.rows;
-               mDepthData.stride = mDispMat.elemSize() *  mDispMat.cols;
+               /* calculate real depth from instrinsic */
+               // mDisp2DeptMat = fx * baseline
+               // depth = fx * baseline / (disparity + offset)
+               // convert depth from 32F to 16U
+               cv::Mat deptF = mDisp2DepthMat.mul(1.0/(mDispMat + mDispOffsetMat));
+               deptF.convertTo(mDepthMat, CV_16U);
+
+               mDepthData.data = mDepthMat.ptr<unsigned short>();
+               mDepthData.type = DFS_DATA_TYPE_UINT16C1;
+               mDepthData.width = mDepthMat.cols;
+               mDepthData.height = mDepthMat.rows;
+               mDepthData.stride = mDepthMat.elemSize() *  mDepthMat.cols;
 
                mDepthData.pointCloudData = nullptr;
                mDepthData.pointCloudSize = 0;