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 MAX_THREADS_NUM 2
  23 #define ENABLE_CALIBRATION 1
  24
  25 namespace DfsAdaptationImpl
  26 {
  27         DfsOCV::DfsOCV() :
  28                 mDfsOcv(nullptr),
  29                 mDfsOcvExtra(nullptr),
  30                 mDfsPostOcv(nullptr),
  31                 mDfsParam(),
  32                 mNumDisparities(64),
  33                 mBlockSize(5),
  34                 mMinDisparity(32),
  35                 mP1(24*1),
  36                 mP2(96*1),
  37                 mPreFilterCap(63),
  38                 mDispMat(),
  39                 mDepthData(),
  40                 mStereoParam(),
  41                 mIsStereoCalibrated(false),
  42                 mUpdateStereoCalibration(false),
  43                 mImageSize(cv::Size(0,0)),
  44                 mDownScale(1),
  45                 mIsStopAll(false)
  46         {
  47                 LOGI("ENTER");
  48                 LOGI("LEAVE");
  49         }
  50
  51         DfsOCV::~DfsOCV()
  52         {
  53                 LOGI("ENTER");
  54                 mIsStopAll = true;
  55                 mControlJob.notify_all();
  56
  57                 for (auto& t : mThreadPool) {
  58                         LOGI("thread joinable: %s", t.joinable() ? "true" : "false");
  59                         if (t.joinable())
  60                                 t.join();
  61                 }
  62                 LOGI("LEAVE");
  63         }
  64
  65         void DfsOCV::InitializeStereoCalibration(std::string filepath)
  66         {
  67                 LOGI("ENTER");
  68
  69                 cv::Mat intrinsicTest;
  70                 try {
  71                         cv::FileStorage fs(filepath, cv::FileStorage::READ);
  72                         if (!fs.isOpened()) {
  73                                 LOGE("Failed to open calib file %s", filepath.c_str());
  74                                 throw std::ios_base::failure("calibration");
  75                         }
  76
  77                         fs["LEFT_CAM_INTRINSIC"] >> mStereoParam.baceCamera.intrinsic;
  78                         fs["LEFT_CAM_DISTCOEFFS"] >> mStereoParam.baceCamera.distCoeffs;
  79
  80                         fs["RIGHT_CAM_INTRINSIC"] >> mStereoParam.extraCamera.intrinsic;
  81                         fs["RIGHT_CAM_DISTCOEFFS"] >> mStereoParam.extraCamera.distCoeffs;
  82
  83                         fs["STEREO_TRANSLATION"] >> mStereoParam.translation;
  84                         fs["STEREO_ROTATION"] >> mStereoParam.rotation;
  85
  86                 } catch (const std::exception& e) {
  87                         LOGE("Failed to read calibration data %s", e.what());
  88                         throw std::ios_base::failure("calibration");
  89                 }
  90
  91                 LOGI("LEAVE");
  92         }
  93
  94         void DfsOCV::InitRectifyMap()
  95         {
  96                 LOGI("ENTER");
  97
  98                 try {
  99                         cv::Mat newBaseCameraIntrinsic = cv::getOptimalNewCameraMatrix(
 100                                                                                                 mStereoParam.baceCamera.intrinsic,
 101                                                                                                 mStereoParam.baceCamera.distCoeffs,
 102                                                                                                 mImageSize,
 103                                                                                                 1.0);
 104
 105                         cv::Mat newExtraCameraIntrinsic = cv::getOptimalNewCameraMatrix(
 106                                                                                                 mStereoParam.extraCamera.intrinsic,
 107                                                                                                 mStereoParam.extraCamera.distCoeffs,
 108                                                                                                 mImageSize,
 109                                                                                                 1.0);
 110
 111                         cv::Mat extraCameraRotation;
 112                         cv::Rodrigues(mStereoParam.rotation, extraCameraRotation);
 113
 114                         cv::Mat baseRotation, extraRotation; // 3x3
 115                         cv::Mat baseProjection, extraProjection; // 3x4
 116                         cv::Mat disp2Depth; // 4x4
 117
 118                         cv::stereoRectify(mStereoParam.baceCamera.intrinsic,
 119                                                 mStereoParam.baceCamera.distCoeffs,
 120                                                 mStereoParam.extraCamera.intrinsic,
 121                                                 mStereoParam.extraCamera.distCoeffs,
 122                                                 mImageSize,
 123                                                 extraCameraRotation,
 124                                                 mStereoParam.translation,
 125                                                 baseRotation,
 126                                                 extraRotation,
 127                                                 baseProjection,
 128                                                 extraProjection,
 129                                                 disp2Depth);
 130
 131                         cv::initUndistortRectifyMap(mStereoParam.baceCamera.intrinsic,
 132                                                                         mStereoParam.baceCamera.distCoeffs,
 133                                                                         baseRotation,
 134                                                                         newBaseCameraIntrinsic,
 135                                                                         mImageSize,
 136                                                                         CV_16SC2,
 137                                                                         mBaseReMap[0],
 138                                                                         mBaseReMap[1]);
 139
 140                         cv::initUndistortRectifyMap(mStereoParam.extraCamera.intrinsic,
 141                                                                         mStereoParam.extraCamera.distCoeffs,
 142                                                                         extraRotation,
 143                                                                         newExtraCameraIntrinsic,
 144                                                                         mImageSize,
 145                                                                         CV_16SC2,
 146                                                                         mExtraReMap[0],
 147                                                                         mExtraReMap[1]);
 148
 149                         mIsStereoCalibrated = true;
 150                 } catch (const std::exception& e) {
 151                         LOGE("Failed to InitRectifyMap");
 152                         throw e;
 153                 }
 154
 155
 156                 LOGI("LEAVE");
 157
 158         }
 159         void DfsOCV::Initialize(DfsParameter& param, size_t width, size_t height,
 160                                                         size_t minDisp, size_t maxDisp, std::string stereoConfigPath)
 161         {
 162                 LOGI("ENTER");
 163
 164                 mDfsParam = param;
 165                 mImageSize = cv::Size(width, height);
 166                 mMinDisparity = minDisp;
 167                 mNumDisparities = static_cast<int>(static_cast<float>(maxDisp - minDisp+1)*mDispShiftInv);
 168                 mNumDisparities *= mDispShift;
 169                 LOGE("mMinDisparity: %zd, mNumDisparities: %zd", mMinDisparity, mNumDisparities);
 170                 if (mDownScale) {
 171                         mMinDisparity >>= mDownScale;
 172                         mNumDisparities >>= mDownScale;
 173                 }
 174                 mDfsOcv = cv::StereoSGBM::create(mMinDisparity,
 175                                                                                 mNumDisparities,
 176                                                                                 mBlockSize,
 177                                                                                 0, // P1
 178                                                                                 0, // P2
 179                                                                                 2, // disp12MaxDiff
 180                                                                                 0, // preFilterCap
 181                                                                                 10, // uniquenessRatio
 182                                                                                 50, // speckleWindowSize
 183                                                                                 2); // speckleRange
 184
 185                 this->SetParameters();
 186
 187 #if ENABLE_CALIBRATION
 188                 if (!stereoConfigPath.empty()) {
 189                         try {
 190                                 this->InitializeStereoCalibration(stereoConfigPath);
 191                                 this->InitRectifyMap();
 192                         } catch (const std::exception& e) {
 193                                 throw e;
 194                         }
 195                 }
 196 #endif
 197                 mDfsPostOcv = cv::ximgproc::createDisparityWLSFilter(mDfsOcv);
 198                 mDfsOcvExtra = cv::ximgproc::createRightMatcher(mDfsOcv);
 199                 mDfsPostOcv->setLRCthresh(24.0);
 200
 201                 mDfsPostOcv->setSigmaColor(1.5);
 202                 mDfsPostOcv->setLambda(8000);
 203                 mDfsPostOcv->setDepthDiscontinuityRadius((int)ceil(0.5*5));
 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("ENTER");
 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("LEAVE");
 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                                 LOGE("baseCvType: %d, extraCvType:%d", baseCvType, extraCvType);
 348                                 throw std::runtime_error("invalid data type");
 349                         }
 350
 351                         baseMat = cv::Mat(cv::Size(base.width, base.height),
 352                                                         baseCvType,
 353                                                         base.data);;
 354                         extraMat = cv::Mat(cv::Size(extra.width,
 355                                                         extra.height),
 356                                                         extraCvType,
 357                                                         extra.data);
 358                 }
 359
 360                 if (baseMat.size() != extraMat.size()) {
 361                         throw std::runtime_error("base and extra should be the same size");
 362                 }
 363
 364                 if (baseMat.type() != extraMat.type()) {
 365                         throw std::runtime_error("base and extra should be the type");
 366                 }
 367
 368                 cv::Mat rBaseMat, rExtraMat, dispMat, dispFiltMat;
 369
 370                 // with remap
 371                 if (mIsStereoCalibrated) {
 372                         cv::remap(baseMat, rBaseMat, mBaseReMap[0], mBaseReMap[1], cv::INTER_LINEAR);
 373                         cv::remap(extraMat, rExtraMat, mExtraReMap[0], mExtraReMap[1], cv::INTER_LINEAR);
 374                 } else {
 375                         rBaseMat = baseMat;
 376                         rExtraMat = extraMat;
 377                 }
 378
 379
 380                 cv::Mat srcBaseMat, srcExtraMat;
 381                 cv::resize(rBaseMat, srcBaseMat,
 382                                         cv::Size(),
 383                                         1.0/static_cast<double>((1<<mDownScale)),
 384                                         1.0/static_cast<double>((1<<mDownScale)));
 385                 cv::resize(rExtraMat, srcExtraMat,
 386                                         cv::Size(),
 387                                         1.0/static_cast<double>((1<<mDownScale)),
 388                                         1.0/static_cast<double>((1<<mDownScale)));
 389
 390                 std::vector<std::future<bool>> results;
 391                 results.emplace_back(EnqueueJob(&DfsOCV::computeL,
 392                                                                                 std::cref(srcBaseMat),
 393                                                                                 std::cref(srcExtraMat),
 394                                                                                 std::ref(dispMat)));
 395                 if (mDfsPostOcv) {
 396                         if (mDfsOcvExtra) {
 397                                 cv::Mat dispMatExtra;
 398                                 results.emplace_back(EnqueueJob(&DfsOCV::computeR,
 399                                                                                 std::cref(srcExtraMat),
 400                                                                                 std::cref(srcBaseMat),
 401                                                                                 std::ref(dispMatExtra)));
 402                                 LOGI("left: %s, right: %s", results[0].get() ? "true" : "false",
 403                                                                                         results[1].get() ? "true" : "false");
 404                                 if (mDownScale) {
 405                                         cv::Mat tmp;
 406                                         // base
 407                                         cv::resize(dispMat, tmp,
 408                                         mImageSize,
 409                                         0.0,
 410                                         0.0);
 411                                         dispMat = tmp * static_cast<double>(1<<mDownScale);
 412
 413                                         // extra
 414                                         cv::resize(dispMatExtra, tmp,
 415                                         mImageSize,
 416                                         0.0,
 417                                         0.0);
 418                                         dispMatExtra = tmp * static_cast<double>(1<<mDownScale);
 419                                 }
 420                                 mDfsPostOcv->filter(dispMat, rBaseMat, dispFiltMat,
 421                                                                         dispMatExtra,
 422                                                                         cv::Rect(0,0,rBaseMat.cols, rBaseMat.rows),
 423                                                                         rExtraMat);
 424                         } else {
 425                                 LOGI("left : %s", results[0].get() ? "true" : "false");
 426                                 if (mDownScale) {
 427                                         cv::Mat tmp;
 428                                         cv::resize(dispMat, tmp,
 429                                         cv::Size(),
 430                                         static_cast<double>(1<<mDownScale),
 431                                         static_cast<double>(1<<mDownScale));
 432                                         dispMat = tmp * static_cast<double>(1<<mDownScale);
 433                                 }
 434                                 mDfsPostOcv->filter(dispMat, rBaseMat, dispFiltMat);
 435                         }
 436                         dispFiltMat.convertTo(mDispMat, CV_32F, mDispShiftInv);
 437                 } else {
 438                         LOGI("left : %s", results[0].get() ? "true" : "false");
 439                         if (mDownScale) {
 440                                 cv::Mat tmp;
 441                                 cv::resize(dispMat, tmp,
 442                                         cv::Size(),
 443                                         static_cast<double>(1<<mDownScale),
 444                                         static_cast<double>(1<<mDownScale));
 445
 446                                 tmp.convertTo(mDispMat, CV_32F, mDispShiftInv * static_cast<double>(1 << mDownScale));
 447                         } else {
 448                                 dispMat.convertTo(mDispMat, CV_32F, mDispShiftInv);
 449                         }
 450                 }
 451
 452                 mDepthData.data = mDispMat.ptr<float>();
 453                 mDepthData.type = DFS_DATA_TYPE_FLOAT32C1;
 454                 mDepthData.width = mDispMat.cols;
 455                 mDepthData.height = mDispMat.rows;
 456                 mDepthData.stride = mDispMat.elemSize() *  mDispMat.cols;
 457
 458                 mDepthData.pointCloudData = nullptr;
 459                 mDepthData.pointCloudSize = 0;
 460
 461                 LOGI("LEAVE");
 462         }
 463
 464         DfsData& DfsOCV::GetDepthData()
 465         {
 466                 LOGI("ENTER");
 467
 468                 return mDepthData;
 469
 470                 LOGI("LEAVE");
 471         }
 472
 473         extern "C"
 474         {
 475                 class IDfsAdaptation *AdaptorInit(void)
 476                 {
 477                         //InferenceTFLite *engine = new InferenceTFLite();
 478                         DfsOCV *adaptor = new DfsOCV();
 479                         return adaptor;
 480                 }
 481
 482                 void AdaptorDestroy(class IDfsAdaptation *adaptor)
 483                 {
 484                         delete adaptor;
 485                 }
 486         }
 487 }