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