src/dfs_opencv.cpp

   1 /**
   2  * Copyright (c) 2021 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 #include <opencv2/imgcodecs.hpp>
  21
  22 #define DEFAULT_STEREO_CALIB_FILE_NAME "stereoCalibZed.yaml"
  23 #define DEFAULT_STEREO_VGA_CALIB_FILE_NAME "stereoCalibZedVGA.yaml"
  24 #define DEFAULT_STEREO_HD_CALIB_FILE_NAME "stereoCalibZedHD.yaml"
  25
  26 #define MAX_THREADS_NUM 2
  27 #define ENABLE_CALIBRATION 0
  28
  29 namespace DfsAdaptationImpl
  30 {
  31         DfsOCV::DfsOCV() :
  32                 mDfsOcv(nullptr),
  33                 mDfsOcvExtra(nullptr),
  34                 mDfsPostOcv(nullptr),
  35                 mDfsParam(),
  36                 mNumDisparities(64),
  37                 mBlockSize(5),
  38                 mMinDisparity(32),
  39                 mP1(24*3),
  40                 mP2(96*3),
  41                 mPreFilterCap(63),
  42                 mDispMat(),
  43                 mDepthData(),
  44                 mCalibFilePath(DFS_CALIB_FILE_PATH),
  45                 mStereoParam(),
  46                 mIsStereoCalibrated(false),
  47                 mUpdateStereoCalibration(false),
  48                 mImageSize(cv::Size(0,0)),
  49                 mDownScale(1),
  50                 mIsStopAll(false)
  51         {
  52                 LOGI("ENTER");
  53                 LOGI("LEAVE");
  54         }
  55
  56         DfsOCV::~DfsOCV()
  57         {
  58                 LOGI("ENTER");
  59                 mIsStopAll = true;
  60                 mControlJob.notify_all();
  61
  62                 for (auto& t : mThreadPool) {
  63                         LOGI("thread joinable: %s", t.joinable() ? "true" : "false");
  64                         if (t.joinable())
  65                                 t.join();
  66                 }
  67                 LOGI("LEAVE");
  68         }
  69
  70         void DfsOCV::InitializeStereoCalibration()
  71         {
  72                 LOGI("ENTER");
  73
  74                 cv::Mat intrinsicTest;
  75                 try {
  76                         if (mImageSize == cv::Size(1280, 720))
  77                                 mCalibFilePath += std::string(DEFAULT_STEREO_HD_CALIB_FILE_NAME);
  78                         else if (mImageSize == cv::Size(672, 376)) {
  79                                 mCalibFilePath += std::string(DEFAULT_STEREO_VGA_CALIB_FILE_NAME);
  80                         } else {
  81                                 throw std::ios_base::failure("no calibration file");
  82                         }
  83
  84                         cv::FileStorage fs(mCalibFilePath, cv::FileStorage::READ);
  85                         if (!fs.isOpened()) {
  86                                 LOGE("Failed to open calib file %s", mCalibFilePath.c_str());
  87                                 throw std::ios_base::failure("calibration");
  88                         }
  89
  90                         fs["LEFT_CAM_INTRINSIC"] >> mStereoParam.baceCamera.intrinsic;
  91                         fs["LEFT_CAM_DISTCOEFFS"] >> mStereoParam.baceCamera.distCoeffs;
  92
  93                         fs["RIGHT_CAM_INTRINSIC"] >> mStereoParam.extraCamera.intrinsic;
  94                         fs["RIGHT_CAM_DISTCOEFFS"] >> mStereoParam.extraCamera.distCoeffs;
  95
  96                         fs["STEREO_TRANSLATION"] >> mStereoParam.translation;
  97                         fs["STEREO_ROTATION"] >> mStereoParam.rotation;
  98
  99                         mIsStereoCalibrated = true;
 100
 101                 } catch (const std::exception& e) {
 102                         LOGE("Failed to read calibration data %s", e.what());
 103                         throw std::ios_base::failure("calibration");
 104                 }
 105
 106                 LOGI("LEAVE");
 107         }
 108
 109         void DfsOCV::InitRectifyMap()
 110         {
 111                 LOGI("ENTER");
 112
 113                 try {
 114                 cv::Mat newBaseCameraIntrinsic = cv::getOptimalNewCameraMatrix(
 115                                                                                                 mStereoParam.baceCamera.intrinsic,
 116                                                                                                 mStereoParam.baceCamera.distCoeffs,
 117                                                                                                 mImageSize,
 118                                                                                                 1.0);
 119
 120                 cv::Mat newExtraCameraIntrinsic = cv::getOptimalNewCameraMatrix(
 121                                                                                                 mStereoParam.extraCamera.intrinsic,
 122                                                                                                 mStereoParam.extraCamera.distCoeffs,
 123                                                                                                 mImageSize,
 124                                                                                                 1.0);
 125
 126                 cv::Mat extraCameraRotation;
 127                 cv::Rodrigues(mStereoParam.rotation, extraCameraRotation);
 128
 129                 cv::Mat baseRotation, extraRotation; // 3x3
 130                 cv::Mat baseProjection, extraProjection; // 3x4
 131                 cv::Mat disp2Depth; // 4x4
 132
 133                 cv::stereoRectify(mStereoParam.baceCamera.intrinsic,
 134                                                 mStereoParam.baceCamera.distCoeffs,
 135                                                 mStereoParam.extraCamera.intrinsic,
 136                                                 mStereoParam.extraCamera.distCoeffs,
 137                                                 mImageSize,
 138                                                 extraCameraRotation,
 139                                                 mStereoParam.translation,
 140                                                 baseRotation,
 141                                                 extraRotation,
 142                                                 baseProjection,
 143                                                 extraProjection,
 144                                                 disp2Depth);
 145
 146                 cv::initUndistortRectifyMap(mStereoParam.baceCamera.intrinsic,
 147                                                                         mStereoParam.baceCamera.distCoeffs,
 148                                                                         baseRotation,
 149                                                                         newBaseCameraIntrinsic,
 150                                                                         mImageSize,
 151                                                                         CV_16SC2,
 152                                                                         mBaseReMap[0],
 153                                                                         mBaseReMap[1]);
 154
 155                 cv::initUndistortRectifyMap(mStereoParam.extraCamera.intrinsic,
 156                                                                         mStereoParam.extraCamera.distCoeffs,
 157                                                                         extraRotation,
 158                                                                         newExtraCameraIntrinsic,
 159                                                                         mImageSize,
 160                                                                         CV_16SC2,
 161                                                                         mExtraReMap[0],
 162                                                                         mExtraReMap[1]);
 163
 164                 } catch (const std::exception& e) {
 165                         LOGE("Failed to InitRectifyMap");
 166                         throw e;
 167                 }
 168
 169
 170                 LOGI("LEAVE");
 171
 172         }
 173         void DfsOCV::Initialize(DfsParameter& param, size_t width, size_t height,
 174                                                         size_t minDisp, size_t maxDisp)
 175         {
 176                 LOGI("ENTER");
 177
 178                 mDfsParam = param;
 179                 mImageSize = cv::Size(width, height);
 180                 mMinDisparity = minDisp;
 181                 mNumDisparities = maxDisp - minDisp + 1;
 182                 if (mDownScale) {
 183                         mMinDisparity >>= mDownScale;
 184                         mNumDisparities >>= mDownScale;
 185                 }
 186                 mDfsOcv = cv::StereoSGBM::create(mMinDisparity, mNumDisparities, mBlockSize);
 187
 188                 this->SetParameters();
 189
 190 #if ENABLE_CALIBRATION
 191                 try {
 192                         this->InitializeStereoCalibration();
 193                         this->InitRectifyMap();
 194                 } catch (const std::exception& e) {
 195                         throw e;
 196                 }
 197 #endif
 198                 mDfsPostOcv = cv::ximgproc::createDisparityWLSFilter(mDfsOcv);
 199                 mDfsOcvExtra = cv::ximgproc::createRightMatcher(mDfsOcv);
 200                 mDfsPostOcv->setLRCthresh(24.0);
 201
 202                 mDfsPostOcv->setSigmaColor(10.0);
 203                 mDfsPostOcv->setLambda(8000);
 204
 205                 if (!mDfsOcvExtra) {
 206                         mThreadPool.reserve(MAX_THREADS_NUM - 1);
 207                         mThreadPool.emplace_back([this](){this->Runner();});
 208                 } else {
 209                         mThreadPool.reserve(MAX_THREADS_NUM);
 210                         mThreadPool.emplace_back([this](){this->Runner();});
 211                         mThreadPool.emplace_back([this](){this->Runner();});
 212                 }
 213                 LOGI("LEAVE");
 214         }
 215
 216         void DfsOCV::SetParameters()
 217         {
 218                 LOGI("ENTER");
 219
 220                 mDfsOcv->setMinDisparity(mMinDisparity);
 221                 mDfsOcv->setNumDisparities(mNumDisparities);
 222                 mDfsOcv->setBlockSize(mBlockSize);
 223                 mDfsOcv->setP1(mP1 * mBlockSize * mBlockSize);
 224                 mDfsOcv->setP2(mP2 * mBlockSize * mBlockSize);
 225                 mDfsOcv->setPreFilterCap(mPreFilterCap);
 226
 227                 mDfsOcv->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);
 228                 LOGI("LEAVE");
 229         }
 230
 231         int DfsOCV::ConvertDfsDataTypeToCV(int type)
 232         {
 233                 LOGI("LEAVE");
 234                 switch (type) {
 235                 case DFS_DATA_TYPE_UINT8C1:
 236                         return CV_8UC1;
 237                 case DFS_DATA_TYPE_UINT8C3:
 238                         return CV_8UC3;
 239                 default:
 240                         LOGE("Invalide type");
 241                 }
 242
 243                 return -1;
 244
 245                 LOGI("ENTER");
 246         }
 247
 248         bool DfsOCV::computeL(const cv::Mat& baseMat, const cv::Mat& extraMat, cv::Mat& disp)
 249         {
 250                 LOGI("ENTER");
 251
 252                 mDfsOcv->compute(baseMat, extraMat, disp);
 253
 254                 LOGI("LEAVE");
 255                 return true;
 256         }
 257
 258         bool DfsOCV::computeR(const cv::Mat& baseMat, const cv::Mat& extraMat, cv::Mat& disp)
 259         {
 260                 LOGI("ENTER");
 261
 262                 mDfsOcvExtra->compute(baseMat, extraMat, disp);
 263
 264                 LOGI("LEAVE");
 265                 return true;
 266         }
 267
 268         void DfsOCV::Runner()
 269         {
 270                 while (true) {
 271                         std::unique_lock<std::mutex> lock(mMutexJob);
 272                         mControlJob.wait(lock, [this]() {return !mJobs.empty() || mIsStopAll;});
 273                         if (mIsStopAll && mJobs.empty()) {
 274                                 LOGI("Stop and all jobs are done");
 275                                 return;
 276                         }
 277
 278                         std::function<void()> job = std::move(mJobs.front());
 279                         mJobs.pop();
 280                         lock.unlock();
 281
 282                         job();
 283                 }
 284         }
 285
 286         template <class F, class... Args>
 287         std::future<bool> DfsOCV::EnqueueJob(F f, Args... args)
 288         {
 289                 LOGE("ENTER");
 290                 if (mIsStopAll) {
 291                         LOGE("Thread pool stopped");
 292                         throw std::runtime_error("thread pool stopped");
 293                 }
 294
 295                 auto job = std::make_shared<std::packaged_task<bool()>>(
 296                                         std::bind(f, this, args...));
 297
 298                 std::future<bool> results = job->get_future();
 299                 {
 300                         std::lock_guard<std::mutex> lock(mMutexJob);
 301                         mJobs.push([job]() { (*job)(); });
 302                 }
 303                 mControlJob.notify_one();
 304
 305                 LOGE("LEAVE");
 306                 return results;
 307         }
 308
 309         void DfsOCV::Run(DfsData& base, DfsData& extra)
 310         {
 311                 LOGI("ENTER");
 312
 313                 if (!base.data) {
 314                         throw std::runtime_error("invalid data pointer");
 315                 }
 316
 317                 int baseCvType = 1;
 318                 int extraCvType = -1;
 319                 cv::Mat baseMat, extraMat;
 320
 321                 if (!extra.data) {
 322                         LOGI("side-by-side");
 323                         if (cv::Size(base.width >> 1, base.height) != mImageSize) {
 324                                 throw std::runtime_error("invalid size");
 325                         }
 326
 327                         baseCvType = ConvertDfsDataTypeToCV(base.type);
 328                         if (baseCvType < 0) {
 329                                 throw std::runtime_error("invalid data type");
 330                         }
 331                         cv::Mat mat(cv::Size(base.width, base.height), baseCvType, base.data);
 332                         LOGI("%zd x %zd", base.width, base.height);
 333                         baseMat = mat(cv::Rect(0, 0,
 334                                                 mImageSize.width,
 335                                                 mImageSize.height)).clone();
 336                         extraMat = mat(cv::Rect(mImageSize.width, 0,
 337                                                 mImageSize.width,
 338                                                 mImageSize.height)).clone();
 339                 } else {
 340                         if (cv::Size(base.width, base.height) != mImageSize ||
 341                                 cv::Size(extra.width, extra.height) != mImageSize) {
 342                                 throw std::runtime_error("invalid size");
 343                         }
 344                         baseCvType = ConvertDfsDataTypeToCV(base.type);
 345                         extraCvType = ConvertDfsDataTypeToCV(extra.type);
 346                         if (baseCvType < 0 || extraCvType < 0) {
 347                                 throw std::runtime_error("invalid data type");
 348                         }
 349
 350                         baseMat = cv::Mat(cv::Size(base.width, base.height),
 351                                                         baseCvType,
 352                                                         base.data);;
 353                         extraMat = cv::Mat(cv::Size(extra.width,
 354                                                         extra.height),
 355                                                         extraCvType,
 356                                                         extra.data);
 357                 }
 358
 359                 if (baseMat.size() != extraMat.size()) {
 360                         throw std::runtime_error("base and extra should be the same size");
 361                 }
 362
 363                 if (baseMat.type() != extraMat.type()) {
 364                         throw std::runtime_error("base and extra should be the type");
 365                 }
 366
 367                 cv::Mat rBaseMat, rExtraMat, dispMat, dispFiltMat;
 368
 369                 // with remap
 370 #if ENABLE_CALIBRATION
 371                 cv::remap(baseMat, rBaseMat, mBaseReMap[0], mBaseReMap[1], cv::INTER_LINEAR);
 372                 cv::remap(extraMat, rExtraMat, mExtraReMap[0], mExtraReMap[1], cv::INTER_LINEAR);
 373 #else
 374                 rBaseMat = baseMat;
 375                 rExtraMat = extraMat;
 376 #endif
 377
 378                 cv::Mat srcBaseMat, srcExtraMat;
 379                 cv::resize(rBaseMat, srcBaseMat,
 380                                         cv::Size(),
 381                                         1.0/static_cast<double>((1<<mDownScale)),
 382                                         1.0/static_cast<double>((1<<mDownScale)));
 383                 cv::resize(rExtraMat, srcExtraMat,
 384                                         cv::Size(),
 385                                         1.0/static_cast<double>((1<<mDownScale)),
 386                                         1.0/static_cast<double>((1<<mDownScale)));
 387
 388                 std::vector<std::future<bool>> results;
 389                 results.emplace_back(EnqueueJob(&DfsOCV::computeL,
 390                                                                                 std::cref(srcBaseMat),
 391                                                                                 std::cref(srcExtraMat),
 392                                                                                 std::ref(dispMat)));
 393                 if (mDfsPostOcv) {
 394                         if (mDfsOcvExtra) {
 395                                 cv::Mat dispMatExtra;
 396                                 results.emplace_back(EnqueueJob(&DfsOCV::computeR,
 397                                                                                 std::cref(srcExtraMat),
 398                                                                                 std::cref(srcBaseMat),
 399                                                                                 std::ref(dispMatExtra)));
 400                                 LOGI("left: %s, right: %s", results[0].get() ? "true" : "false",
 401                                                                                         results[1].get() ? "true" : "false");
 402                                 if (mDownScale) {
 403                                         cv::Mat tmp;
 404                                         // base
 405                                         cv::resize(dispMat, tmp,
 406                                         mImageSize,
 407                                         0.0,
 408                                         0.0);
 409                                         dispMat = tmp * static_cast<double>(1<<mDownScale);
 410
 411                                         // extra
 412                                         cv::resize(dispMatExtra, tmp,
 413                                         mImageSize,
 414                                         0.0,
 415                                         0.0);
 416                                         dispMatExtra = tmp * static_cast<double>(1<<mDownScale);
 417                                 }
 418                                 mDfsPostOcv->filter(dispMat, rBaseMat, dispFiltMat,
 419                                                                         dispMatExtra,
 420                                                                         cv::Rect(0,0,rBaseMat.cols, rBaseMat.rows),
 421                                                                         rExtraMat);
 422                         } else {
 423                                 LOGI("left : %s", results[0].get() ? "true" : "false");
 424                                 if (mDownScale) {
 425                                         cv::Mat tmp;
 426                                         cv::resize(dispMat, tmp,
 427                                         cv::Size(),
 428                                         static_cast<double>(1<<mDownScale),
 429                                         static_cast<double>(1<<mDownScale));
 430                                         dispMat = tmp * static_cast<double>(1<<mDownScale);
 431                                 }
 432                                 mDfsPostOcv->filter(dispMat, rBaseMat, dispFiltMat);
 433                         }
 434                         dispFiltMat.convertTo(mDispMat, CV_8UC1, 1.0/16.0);
 435                 } else {
 436                         LOGI("left : %s", results[0].get() ? "true" : "false");
 437                         if (mDownScale) {
 438                                 cv::Mat tmp;
 439                                 cv::resize(dispMat, tmp,
 440                                         cv::Size(),
 441                                         static_cast<double>(1<<mDownScale),
 442                                         static_cast<double>(1<<mDownScale));
 443
 444                                 tmp.convertTo(mDispMat, CV_8UC1, (1.0/16.0) * static_cast<double>(1 << mDownScale));
 445                         } else {
 446                                 dispMat.convertTo(mDispMat, CV_8UC1, 1.0/16.0);
 447                         }
 448                 }
 449
 450                 mDepthData.data = mDispMat.ptr<unsigned char>();
 451                 mDepthData.type = DFS_DATA_TYPE_UINT8C1;
 452                 mDepthData.width = mDispMat.cols;
 453                 mDepthData.height = mDispMat.rows;
 454                 mDepthData.stride = mDispMat.elemSize() *  mDispMat.cols;
 455
 456                 LOGI("LEAVE");
 457         }
 458
 459         DfsData& DfsOCV::GetDepthData()
 460         {
 461                 LOGI("ENTER");
 462
 463                 return mDepthData;
 464
 465                 LOGI("LEAVE");
 466         }
 467
 468         extern "C"
 469         {
 470                 class IDfsAdaptation *AdaptorInit(void)
 471                 {
 472                         //InferenceTFLite *engine = new InferenceTFLite();
 473                         DfsOCV *adaptor = new DfsOCV();
 474                         return adaptor;
 475                 }
 476
 477                 void AdaptorDestroy(class IDfsAdaptation *adaptor)
 478                 {
 479                         delete adaptor;
 480                 }
 481         }
 482 }