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(640, 480)
  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)
 149         {
 150                 LOGI("ENTER");
 151
 152                 mDfsParam = param;
 153
 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         void DfsOCV::Run(DfsData& base, DfsData& extra)
 198         {
 199                 LOGI("ENTER");
 200
 201                 if (!base.data || !extra.data) {
 202                         throw std::runtime_error("invalid data pointer");
 203                 }
 204
 205                 int baseCvType = ConvertDfsDataTypeToCV(base.type);
 206                 int extraCvType = ConvertDfsDataTypeToCV(extra.type);
 207                 if (baseCvType < 0 || extraCvType < 0) {
 208                         throw std::runtime_error("invalid data type");
 209                 }
 210
 211                 cv::Mat baseMat(cv::Size(base.width, base.height), baseCvType, base.data);
 212                 cv::Mat extraMat(cv::Size(extra.width, extra.height), extraCvType, extra.data);
 213
 214                 if (baseMat.size() != extraMat.size()) {
 215                         throw std::runtime_error("base and extra should be the same size");
 216                 }
 217
 218                 if (baseMat.type() != extraMat.type()) {
 219                         throw std::runtime_error("base and extra should be the type");
 220                 }
 221
 222                 cv::Mat rBaseMat, rExtraMat;
 223                 cv::remap(baseMat, rBaseMat, mBaseReMap[0], mBaseReMap[1], cv::INTER_LINEAR);
 224                 cv::remap(extraMat, rExtraMat, mExtraReMap[0], mExtraReMap[1], cv::INTER_LINEAR);
 225
 226                 mDfsOcv->compute(rBaseMat, rExtraMat, mDispMat);
 227
 228                 mDepthData.data = mDispMat.data;
 229                 mDepthData.type = DFS_DATA_TYPE_UINT8;
 230                 mDepthData.width = mDispMat.cols;
 231                 mDepthData.height = mDispMat.rows;
 232                 mDepthData.stride = mDispMat.elemSize();
 233
 234                 LOGI("LEAVE");
 235         }
 236
 237         DfsData& DfsOCV::GetDepthData()
 238         {
 239                 LOGI("ENTER");
 240
 241                 return mDepthData;
 242
 243                 LOGI("LEAVE");
 244         }
 245
 246         extern "C"
 247         {
 248                 class IDfsAdaptation *AdaptorInit(void)
 249                 {
 250                         //InferenceTFLite *engine = new InferenceTFLite();
 251                         DfsOCV *adaptor = new DfsOCV();
 252                         return adaptor;
 253                 }
 254
 255                 void AdaptorDestroy(class IDfsAdaptation *adaptor)
 256                 {
 257                         delete adaptor;
 258                 }
 259         }
 260 }