src/dfs_opencv.cpp

   1 /**
   2  * Copyright (c) 2019 Samsung Electronics Co., Ltd All Rights Reserved
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 #include "dfs_opencv_private.h"
  18 #include <dlog.h>
  19 #include <opencv2/core/persistence.hpp>
  20
  21 #define DEFAULT_STEREO_CALIB_FILE_NAME "stereoCalibZed.yaml"
  22
  23 namespace DfsAdaptationImpl
  24 {
  25         DfsOCV::DfsOCV() :
  26                 mDfsOcv(nullptr),
  27                 mDfsParam(),
  28                 mNumDisparities(179),
  29                 mBlockSize(5),
  30                 mMinDisparity(32),
  31                 mP1(24*3),
  32                 mP2(96*3),
  33                 mPreFilterCap(63),
  34                 mDispMat(),
  35                 mDepthData(),
  36                 mCalibFilePath(DFS_CALIB_FILE_PATH),
  37                 mStereoParam(),
  38                 mIsStereoCalibrated(false),
  39                 mUpdateStereoCalibration(false),
  40                 mImageSize(cv::Size(0,0))
  41         {
  42                 LOGI("ENTER");
  43                 mCalibFilePath += std::string(DEFAULT_STEREO_CALIB_FILE_NAME);
  44                 LOGI("LEAVE");
  45         }
  46
  47         DfsOCV::~DfsOCV()
  48         {
  49                 LOGI("ENTER");
  50                 LOGI("LEAVE");
  51         }
  52
  53         void DfsOCV::InitializeStereoCalibration()
  54         {
  55                 LOGI("ENTER");
  56
  57                 cv::Mat intrinsicTest;
  58                 try {
  59                         cv::FileStorage fs(mCalibFilePath, cv::FileStorage::READ);
  60                         if (!fs.isOpened()) {
  61                                 LOGE("Failed to open calib file %s", mCalibFilePath.c_str());
  62                                 throw std::ios_base::failure("calibration");
  63                         }
  64
  65                         fs["LEFT_CAM_VGA"] >> mStereoParam.baceCamera.intrinsic;
  66                         fs["LEFT_CAM_DISTCOEFFS"] >> mStereoParam.baceCamera.distCoeffs;
  67
  68                         fs["RIGHT_CAM_VGA"] >> mStereoParam.extraCamera.intrinsic;
  69                         fs["RIGHT_CAM_DISTCOEFFS"] >> mStereoParam.extraCamera.distCoeffs;
  70
  71                         fs["STEREO_TRANSLATION"] >> mStereoParam.translation;
  72                         fs["STEREO_ROTATION"] >> mStereoParam.rotation;
  73
  74                         mIsStereoCalibrated = true;
  75
  76                 } catch (const std::exception& e) {
  77                         LOGE("Failed to read calibration data %s", e.what());
  78                         throw std::ios_base::failure("calibration");
  79                 }
  80
  81                 LOGI("LEAVE");
  82         }
  83
  84         void DfsOCV::InitRectifyMap()
  85         {
  86                 LOGI("ENTER");
  87
  88                 try {
  89                 cv::Mat newBaseCameraIntrinsic = cv::getOptimalNewCameraMatrix(
  90                                                                                                 mStereoParam.baceCamera.intrinsic,
  91                                                                                                 mStereoParam.baceCamera.distCoeffs,
  92                                                                                                 mImageSize,
  93                                                                                                 1.0);
  94
  95                 cv::Mat newExtraCameraIntrinsic = cv::getOptimalNewCameraMatrix(
  96                                                                                                 mStereoParam.extraCamera.intrinsic,
  97                                                                                                 mStereoParam.extraCamera.distCoeffs,
  98                                                                                                 mImageSize,
  99                                                                                                 1.0);
 100
 101                 cv::Mat extraCameraRotation;
 102                 cv::Rodrigues(mStereoParam.rotation, extraCameraRotation);
 103
 104                 cv::Mat baseRotation, extraRotation; // 3x3
 105                 cv::Mat baseProjection, extraProjection; // 3x4
 106                 cv::Mat disp2Depth; // 4x4
 107
 108                 cv::stereoRectify(mStereoParam.baceCamera.intrinsic,
 109                                                 mStereoParam.baceCamera.distCoeffs,
 110                                                 mStereoParam.extraCamera.intrinsic,
 111                                                 mStereoParam.extraCamera.distCoeffs,
 112                                                 mImageSize,
 113                                                 extraCameraRotation,
 114                                                 mStereoParam.translation,
 115                                                 baseRotation,
 116                                                 extraRotation,
 117                                                 baseProjection,
 118                                                 extraProjection,
 119                                                 disp2Depth);
 120
 121                 cv::initUndistortRectifyMap(mStereoParam.baceCamera.intrinsic,
 122                                                                         mStereoParam.baceCamera.distCoeffs,
 123                                                                         baseRotation,
 124                                                                         newBaseCameraIntrinsic,
 125                                                                         mImageSize,
 126                                                                         CV_16SC2,
 127                                                                         mBaseReMap[0],
 128                                                                         mBaseReMap[1]);
 129
 130                 cv::initUndistortRectifyMap(mStereoParam.extraCamera.intrinsic,
 131                                                                         mStereoParam.extraCamera.distCoeffs,
 132                                                                         extraRotation,
 133                                                                         newExtraCameraIntrinsic,
 134                                                                         mImageSize,
 135                                                                         CV_16SC2,
 136                                                                         mExtraReMap[0],
 137                                                                         mExtraReMap[1]);
 138
 139                 } catch (const std::exception& e) {
 140                         LOGE("Failed to InitRectifyMap");
 141                         throw e;
 142                 }
 143
 144
 145                 LOGI("LEAVE");
 146
 147         }
 148         void DfsOCV::Initialize(DfsParameter& param, size_t width, size_t height)
 149         {
 150                 LOGI("ENTER");
 151
 152                 mDfsParam = param;
 153                 mImageSize = cv::Size(width, height);
 154                 mDfsOcv = cv::StereoSGBM::create(1, mNumDisparities, mBlockSize);
 155
 156                 this->SetParameters();
 157
 158                 try {
 159                         this->InitializeStereoCalibration();
 160                         this->InitRectifyMap();
 161                 } catch (const std::exception& e) {
 162                         throw e;
 163                 }
 164
 165                 LOGI("LEAVE");
 166         }
 167
 168         void DfsOCV::SetParameters()
 169         {
 170                 LOGI("ENTER");
 171
 172                 mDfsOcv->setMinDisparity(mMinDisparity);
 173                 mDfsOcv->setNumDisparities(mNumDisparities);
 174                 mDfsOcv->setBlockSize(mBlockSize);
 175                 mDfsOcv->setP1(mP1 * mBlockSize * mBlockSize);
 176                 mDfsOcv->setP2(mP2 * mBlockSize * mBlockSize);
 177                 mDfsOcv->setPreFilterCap(mPreFilterCap);
 178
 179                 mDfsOcv->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);
 180                 LOGI("LEAVE");
 181         }
 182
 183         int DfsOCV::ConvertDfsDataTypeToCV(int type)
 184         {
 185                 LOGI("LEAVE");
 186                 switch (type) {
 187                 case DFS_DATA_TYPE_UINT8:
 188                         return CV_8UC1;
 189                 default:
 190                         LOGE("Invalide type");
 191                 }
 192
 193                 return -1;
 194
 195                 LOGI("ENTER");
 196         }
 197
 198         void DfsOCV::Run(DfsData& base, DfsData& extra)
 199         {
 200                 LOGI("ENTER");
 201
 202                 if (!base.data || !extra.data) {
 203                         throw std::runtime_error("invalid data pointer");
 204                 }
 205
 206                 if (cv::Size(base.width, base.height) != mImageSize ||
 207                         cv::Size(extra.width, extra.height) != mImageSize) {
 208                         throw std::runtime_error("invalid size");
 209                 }
 210
 211                 int baseCvType = ConvertDfsDataTypeToCV(base.type);
 212                 int extraCvType = ConvertDfsDataTypeToCV(extra.type);
 213                 if (baseCvType < 0 || extraCvType < 0) {
 214                         throw std::runtime_error("invalid data type");
 215                 }
 216
 217                 cv::Mat baseMat(cv::Size(base.width, base.height), baseCvType, base.data);
 218                 cv::Mat extraMat(cv::Size(extra.width, extra.height), extraCvType, extra.data);
 219
 220                 if (baseMat.size() != extraMat.size()) {
 221                         throw std::runtime_error("base and extra should be the same size");
 222                 }
 223
 224                 if (baseMat.type() != extraMat.type()) {
 225                         throw std::runtime_error("base and extra should be the type");
 226                 }
 227
 228                 cv::Mat rBaseMat, rExtraMat, dispMat;
 229                 cv::remap(baseMat, rBaseMat, mBaseReMap[0], mBaseReMap[1], cv::INTER_LINEAR);
 230                 cv::remap(extraMat, rExtraMat, mExtraReMap[0], mExtraReMap[1], cv::INTER_LINEAR);
 231
 232                 mDfsOcv->compute(rBaseMat, rExtraMat, dispMat);
 233                 dispMat.convertTo(mDispMat, CV_8UC1, 1.0/16.0);
 234
 235                 mDepthData.data = mDispMat.data;
 236                 mDepthData.type = DFS_DATA_TYPE_UINT8;
 237                 mDepthData.width = mDispMat.cols;
 238                 mDepthData.height = mDispMat.rows;
 239                 mDepthData.stride = mDispMat.elemSize();
 240
 241                 LOGI("LEAVE");
 242         }
 243
 244         DfsData& DfsOCV::GetDepthData()
 245         {
 246                 LOGI("ENTER");
 247
 248                 return mDepthData;
 249
 250                 LOGI("LEAVE");
 251         }
 252
 253         extern "C"
 254         {
 255                 class IDfsAdaptation *AdaptorInit(void)
 256                 {
 257                         //InferenceTFLite *engine = new InferenceTFLite();
 258                         DfsOCV *adaptor = new DfsOCV();
 259                         return adaptor;
 260                 }
 261
 262                 void AdaptorDestroy(class IDfsAdaptation *adaptor)
 263                 {
 264                         delete adaptor;
 265                 }
 266         }
 267 }