mv_3d/3d/src/Mv3d.cpp

   1 /*
   2  * Copyright (c) 2022 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 "mv_private.h"
  18 #include <mv_common.h>
  19 #include <MediaSource.h>
  20 #include <memory>
  21 #include <unistd.h>
  22 #include <opencv2/core.hpp>
  23 #include <opencv2/imgcodecs.hpp>
  24 #include <opencv2/imgproc.hpp>
  25
  26 #ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
  27 #include <open3d/Open3D.h>
  28 using namespace open3d;
  29 #endif
  30
  31 #include "Mv3d.h"
  32 #define MV_3D_TIMEOUT_SEC 3
  33 namespace mediavision
  34 {
  35 namespace mv3d
  36 {
  37 static void __destroyNotifyCb(gpointer data)
  38 {
  39         if (!data)
  40                 return;
  41
  42         LOGW("Cleanup remained DfsInputData (%p)", data);
  43         auto remained = static_cast<DfsInputData *>(data);
  44
  45         delete[] static_cast<unsigned char *>(remained->data);
  46         delete[] static_cast<unsigned char *>(remained->extraData);
  47
  48         delete remained;
  49 }
  50
  51 Mv3d::Mv3d()
  52                 : mDfsParameter()
  53                 , mDfsAdaptor(nullptr)
  54                 , mMode(MV_3D_DEPTH_MODE_NONE)
  55                 , mWidth(0)
  56                 , mHeight(0)
  57                 , mMinDisp(0)
  58                 , mMaxDisp(0)
  59                 , mSamplingRatio(1.0)
  60                 , mOutlierRemovalPoints(0)
  61                 , mOutlierRemovalRadius(0.0)
  62                 , mDfsThread(nullptr)
  63                 , mDepthUserData(nullptr)
  64                 , mPointcloudUserData(nullptr)
  65                 , mDepthCallback(nullptr)
  66                 , mPointcloudCallback(nullptr)
  67                 , mDfsIsLive(false)
  68                 , mDfsAsyncQueue(nullptr)
  69                 , mInternalSource(nullptr)
  70 {
  71         LOGI("ENTER");
  72 #ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
  73         utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);
  74 #endif
  75         LOGI("LEAVE");
  76 }
  77
  78 Mv3d::~Mv3d()
  79 {
  80         LOGI("ENTER");
  81
  82         if (mDfsThread) {
  83                 mDfsIsLive = false;
  84                 g_thread_join(mDfsThread);
  85         }
  86
  87         if (mDfsAsyncQueue) {
  88                 g_async_queue_unref(mDfsAsyncQueue);
  89         }
  90
  91         if (mDfsAdaptor) {
  92                 mDfsAdaptor->unBind();
  93                 delete mDfsAdaptor;
  94         }
  95
  96         if (mInternalSource) {
  97                 mv_destroy_source(mInternalSource);
  98                 mInternalSource = nullptr;
  99         }
 100
 101         LOGI("LEAVE");
 102 }
 103
 104 void Mv3d::SetParameters(double threshold, size_t windowWidth, size_t windowHeight, size_t speckleSize)
 105 {
 106         mDfsParameter.textureThreshold = threshold;
 107         mDfsParameter.aggregationWindowWidth = windowWidth;
 108         mDfsParameter.aggregationWindowHeight = windowHeight;
 109         mDfsParameter.maxSpeckleSize = speckleSize;
 110 }
 111
 112 int Mv3d::Configure(int mode, unsigned int width, unsigned int height, int minDisp, int maxDisp, double samplingRatio,
 113                                         int outlierRemovalPoints, double outlierRemovalRadius, std::string stereoConfigPath,
 114                                         std::string pointcloudOutputPath)
 115 {
 116         mMode = mode;
 117         mWidth = static_cast<size_t>(width);
 118         mHeight = static_cast<size_t>(height);
 119         mMinDisp = minDisp;
 120         mMaxDisp = maxDisp;
 121         mSamplingRatio = samplingRatio;
 122         mOutlierRemovalPoints = outlierRemovalPoints;
 123         mOutlierRemovalRadius = outlierRemovalRadius;
 124         mStereoConfigPath = stereoConfigPath;
 125         size_t found = stereoConfigPath.rfind(".");
 126         mIntrinsicPath = stereoConfigPath.substr(0, found) + std::string(".json");
 127         mPointcloudOutputPath = pointcloudOutputPath;
 128
 129         if (!mDfsAdaptor) {
 130                 try {
 131                         mDfsAdaptor = new DfsAdaptor();
 132                         mDfsAdaptor->bind();
 133                 } catch (const std::bad_alloc &e) {
 134                         LOGE("Failed to create dfs adaptation : %s", e.what());
 135                         return MEDIA_VISION_ERROR_OUT_OF_MEMORY;
 136                 } catch (const std::runtime_error &e) {
 137                         LOGE("Failed to bind %s adaptor", e.what());
 138                         return MEDIA_VISION_ERROR_INVALID_OPERATION;
 139                 }
 140         }
 141
 142         return MEDIA_VISION_ERROR_NONE;
 143 }
 144
 145 void Mv3d::SetDepthCallback(mv_3d_depth_cb depthCallback, void *depthUserData)
 146 {
 147         mDepthCallback = depthCallback;
 148         mDepthUserData = depthUserData;
 149 }
 150
 151 #ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
 152 int Mv3d::SetPointcloudCallback(mv_3d_pointcloud_cb pointcloudCallback, void *pointcloudUserData)
 153 {
 154         if (pointcloudCallback == NULL || pointcloudUserData == NULL)
 155                 return MEDIA_VISION_ERROR_INVALID_PARAMETER;
 156
 157         mPointcloudCallback = pointcloudCallback;
 158         mPointcloudUserData = pointcloudUserData;
 159
 160         return MEDIA_VISION_ERROR_NONE;
 161 }
 162 #else
 163 int Mv3d::SetPointcloudCallback(mv_3d_pointcloud_cb pointcloudCallback, void *pointcloudUserData)
 164 {
 165         return MEDIA_VISION_ERROR_NOT_SUPPORTED;
 166 }
 167 #endif
 168
 169 int Mv3d::Prepare()
 170 {
 171         if (mMode == MV_3D_DEPTH_MODE_NONE) {
 172                 LOGE("Invalid Operation. Set proper MV_3D_DEPTH_MODE");
 173                 return MEDIA_VISION_ERROR_INVALID_OPERATION;
 174         }
 175
 176         if (!mDfsAdaptor) {
 177                 LOGE("Invalid Operation. Do Configure first.");
 178                 return MEDIA_VISION_ERROR_INVALID_OPERATION;
 179         }
 180
 181         try {
 182                 mDfsAdaptor->initialize(mDfsParameter, mWidth, mHeight, mMinDisp, mMaxDisp, mStereoConfigPath);
 183         } catch (const std::exception &e) {
 184                 LOGE("Failed to initialize");
 185                 return MEDIA_VISION_ERROR_INVALID_OPERATION;
 186         }
 187
 188         return MEDIA_VISION_ERROR_NONE;
 189 }
 190
 191 /*
 192 * Get buffer data from mv_source.
 193 * For now it only support no-stride image, or stride with nv12 format
 194 * TODO: zero-copy, multi-planar image handle
 195 */
 196 void Mv3d::GetBufferFromSource(mv_source_h source, unsigned char *&buffer, unsigned int &width, unsigned int &height,
 197                                                            int &type, size_t &stride)
 198 {
 199         MediaVision::Common::MediaSource *mediaSource = static_cast<MediaVision::Common::MediaSource *>(source);
 200
 201         auto *_buffer = mediaSource->getBuffer();
 202         auto _bufferSize = mediaSource->getBufferSize();
 203         width = mediaSource->getWidth();
 204         height = mediaSource->getHeight();
 205         stride = mediaSource->getWidthStride();
 206
 207         switch (mediaSource->getColorspace()) {
 208         case MEDIA_VISION_COLORSPACE_RGB888:
 209                 type = DFS_DATA_TYPE_UINT8C3;
 210                 break;
 211         case MEDIA_VISION_COLORSPACE_I420:
 212         case MEDIA_VISION_COLORSPACE_NV12:
 213         case MEDIA_VISION_COLORSPACE_NV21:
 214         case MEDIA_VISION_COLORSPACE_Y800:
 215                 type = DFS_DATA_TYPE_UINT8C1;
 216                 break;
 217
 218         default: //NOT tested
 219                 throw std::runtime_error("invalid color space");
 220         }
 221
 222         if (width != stride && type == DFS_DATA_TYPE_UINT8C1) {
 223                 //copy only Y channel
 224                 _bufferSize = width * height;
 225                 buffer = new unsigned char[_bufferSize];
 226                 for (unsigned int i = 0; i < height; i++) {
 227                         memcpy(buffer + i * width, _buffer + i * stride, width);
 228                 }
 229         } else {
 230                 buffer = new unsigned char[_bufferSize];
 231                 memcpy(buffer, _buffer, _bufferSize);
 232         }
 233 }
 234
 235 void Mv3d::GetDfsDataFromSources(mv_source_h baseSource, mv_source_h extraSource, DfsInputData &input)
 236 {
 237         unsigned char *baseBuffer = nullptr;
 238         unsigned char *extraBuffer = nullptr;
 239         unsigned int width = 0;
 240         unsigned int height = 0;
 241         int type = 0;
 242         size_t stride = 0;
 243
 244         GetBufferFromSource(baseSource, baseBuffer, width, height, type, stride);
 245         input.data = static_cast<void *>(baseBuffer);
 246         input.type = type;
 247         input.width = width;
 248         input.height = height;
 249         input.stride = stride;
 250         input.format = (mWidth == width && mHeight * 2 == height) ? DFS_DATA_INPUT_FORMAT_COUPLED_TB :
 251                                                                                                                                 DFS_DATA_INPUT_FORMAT_COUPLED_SBS;
 252
 253         if (extraSource) {
 254                 extraBuffer = nullptr;
 255                 GetBufferFromSource(extraSource, extraBuffer, width, height, type, stride);
 256
 257                 if (input.type != type || input.width != width || input.height != height || input.stride != stride) {
 258                         throw std::runtime_error("left and right image's properties are different");
 259                 }
 260
 261                 input.extraData = static_cast<void *>(extraBuffer);
 262                 input.format = DFS_DATA_INPUT_FORMAT_DECOUPLED_SBS;
 263         }
 264 }
 265
 266 #ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
 267 PointCloudPtr Mv3d::GetPointcloudFromSource(DfsInputData &input, DfsOutputData &depthData)
 268 {
 269         camera::PinholeCameraIntrinsic intrinsic;
 270         io::ReadIJsonConvertible(mIntrinsicPath, intrinsic);
 271         cv::Mat img;
 272         double depth_scale = 1000.0, depth_trunc = 200.0;
 273         if (input.type == DFS_DATA_TYPE_UINT8C1) {
 274                 img = cv::Mat(cv::Size(input.width, input.height), CV_8UC1, input.data);
 275                 cv::cvtColor(img, img, cv::COLOR_GRAY2RGB);
 276         } else if (input.type == DFS_DATA_TYPE_UINT8C3) {
 277                 img = cv::Mat(cv::Size(input.width, input.height), CV_8UC3, input.data);
 278                 cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
 279         }
 280
 281         geometry::Image img_color, img_depth;
 282         img_color.Prepare(input.width, input.height, 3, 1);
 283         uint8_t *pImg_color_data = img_color.data_.data();
 284         memcpy(pImg_color_data, img.data, input.width * input.height * 3 * sizeof(unsigned char));
 285
 286         img_depth.Prepare(input.width, input.height, 1, 2);
 287         uint16_t *pImg_depth_data = (unsigned short *) img_depth.data_.data();
 288         memcpy(pImg_depth_data, static_cast<DepthTypePtr>(depthData.data), depthData.width * depthData.height * 2);
 289
 290         std::shared_ptr<geometry::RGBDImage> rgbd_image =
 291                         geometry::RGBDImage::CreateFromColorAndDepth(img_color, img_depth, depth_scale, depth_trunc, false);
 292
 293         auto pcd = geometry::PointCloud::CreateFromRGBDImage(*rgbd_image, intrinsic);
 294         if (mSamplingRatio < 1.0) {
 295                 utility::LogInfo("Downsampling... {}", mSamplingRatio);
 296                 pcd = pcd->RandomDownSample(mSamplingRatio);
 297         }
 298
 299         if (mOutlierRemovalPoints > 0 && mOutlierRemovalRadius > 0.0) {
 300                 utility::LogInfo("RemoveRadiusOutliers... {} {}", mOutlierRemovalPoints, mOutlierRemovalRadius);
 301                 std::vector<size_t> pt_map;
 302                 std::tie(pcd, pt_map) = pcd->RemoveRadiusOutliers(mOutlierRemovalPoints, mOutlierRemovalRadius, true);
 303         }
 304
 305         return pcd;
 306 }
 307 #endif
 308
 309 int Mv3d::Run(mv_source_h baseSource, mv_source_h extraSource)
 310 {
 311         DfsInputData input;
 312         try {
 313                 if (mDfsThread) {
 314                         mDfsIsLive = false;
 315                         g_thread_join(mDfsThread);
 316                         mDfsThread = nullptr;
 317                 }
 318
 319                 if (mDfsAsyncQueue) {
 320                         g_async_queue_unref(mDfsAsyncQueue);
 321                         mDfsAsyncQueue = nullptr;
 322                 }
 323
 324                 if (mInternalSource) {
 325                         int ret = mv_destroy_source(mInternalSource);
 326                         if (ret != MEDIA_VISION_ERROR_NONE) {
 327                                 LOGE("Fail to destroy intern source. But keep going..");
 328                         }
 329                         mInternalSource = nullptr;
 330                 }
 331
 332                 GetDfsDataFromSources(baseSource, extraSource, input);
 333
 334                 mDfsAdaptor->run(input);
 335                 auto depthData = mDfsAdaptor->getOutputData();
 336
 337                 mDepthCallback(baseSource, static_cast<DepthTypePtr>(depthData.data), depthData.width, depthData.height,
 338                                            mDepthUserData);
 339 #ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
 340                 if (mPointcloudCallback) {
 341                         mv_3d_pointcloud_s p { .type = MV_3D_POINTCLOUD_TYPE_PCD_BIN,
 342                                                                    .pointcloud = GetPointcloudFromSource(input, depthData) };
 343
 344                         mPointcloudCallback(baseSource, static_cast<mv_3d_pointcloud_h>(&p), mPointcloudUserData);
 345                 }
 346 #endif
 347                 delete[] static_cast<unsigned char *>(input.data);
 348                 delete[] static_cast<unsigned char *>(input.extraData);
 349         } catch (const std::exception &e) {
 350                 LOGE("Failed to Run with %s", e.what());
 351                 return MEDIA_VISION_ERROR_INVALID_OPERATION;
 352         }
 353
 354         return MEDIA_VISION_ERROR_NONE;
 355 }
 356
 357 int Mv3d::RunAsync(mv_source_h baseSource, mv_source_h extraSource)
 358 {
 359         try {
 360                 if (!mDfsAsyncQueue) {
 361                         mDfsAsyncQueue = g_async_queue_new_full(__destroyNotifyCb);
 362                         if (!mDfsAsyncQueue) {
 363                                 LOGE("Fail to g_async_queue_new()");
 364                                 return MEDIA_VISION_ERROR_INTERNAL;
 365                         }
 366                 }
 367
 368                 if (!mDfsThread) {
 369                         mDfsThread = g_thread_new("depth_thread", &Mv3d::DfsThreadLoop, static_cast<gpointer>(this));
 370
 371                         if (!mDfsThread) {
 372                                 g_async_queue_unref(mDfsAsyncQueue);
 373                                 mDfsAsyncQueue = nullptr;
 374                                 LOGE("Fail to g_thread_new()");
 375                                 return MEDIA_VISION_ERROR_INTERNAL;
 376                         }
 377                         mDfsIsLive = true;
 378                 }
 379
 380                 auto input = std::make_unique<DfsInputData>();
 381                 GetDfsDataFromSources(baseSource, extraSource, *input);
 382                 if (!mInternalSource) {
 383                         int ret = mv_create_source(&mInternalSource);
 384                         if (ret != MEDIA_VISION_ERROR_NONE) {
 385                                 LOGE("Fail to create internal source");
 386                                 return MEDIA_VISION_ERROR_INTERNAL;
 387                         }
 388                 }
 389                 g_async_queue_push(mDfsAsyncQueue, static_cast<gpointer>(input.release()));
 390         } catch (const std::exception &e) {
 391                 LOGE("Failed to Run with %s", e.what());
 392                 return MEDIA_VISION_ERROR_INVALID_OPERATION;
 393         }
 394
 395         return MEDIA_VISION_ERROR_NONE;
 396 }
 397
 398 gpointer Mv3d::DfsThreadLoop(gpointer data)
 399 {
 400         auto handle = static_cast<Mv3d *>(data);
 401         while (handle->mDfsIsLive) {
 402                 auto base = g_async_queue_timeout_pop(handle->mDfsAsyncQueue, MV_3D_TIMEOUT_SEC * G_TIME_SPAN_SECOND);
 403                 if (!base) {
 404                         LOGW("Timeout! no more items. Exit thread");
 405                         break;
 406                 }
 407
 408                 auto queueRemaining = g_async_queue_length(handle->mDfsAsyncQueue);
 409                 LOGI("%d remaining", queueRemaining);
 410
 411                 while (queueRemaining > 0) {
 412                         auto itemToDelete = static_cast<DfsInputData *>(base);
 413                         delete[] static_cast<unsigned char *>(itemToDelete->data);
 414                         delete[] static_cast<unsigned char *>(itemToDelete->extraData);
 415                         delete itemToDelete;
 416
 417                         base = g_async_queue_pop(handle->mDfsAsyncQueue);
 418                         queueRemaining--;
 419                         LOGI("%d remaining", queueRemaining);
 420                 }
 421
 422                 std::unique_ptr<DfsInputData> pInput(static_cast<DfsInputData *>(base));
 423                 handle->mDfsAdaptor->run(*pInput);
 424
 425                 auto depthData = handle->mDfsAdaptor->getOutputData();
 426                 auto leftData = handle->mDfsAdaptor->getLeftData();
 427                 int ret = mv_source_fill_by_buffer(handle->mInternalSource, static_cast<unsigned char *>(leftData.data),
 428                                                                                    leftData.stride * leftData.height, leftData.width, leftData.height,
 429                                                                                    leftData.type == DFS_DATA_TYPE_UINT8C3 ? MEDIA_VISION_COLORSPACE_RGB888 :
 430                                                                                                                                                                         MEDIA_VISION_COLORSPACE_Y800);
 431                 if (MEDIA_VISION_ERROR_NONE != ret) {
 432                         LOGW("Errors were occurred during source filling %i", ret);
 433                         continue;
 434                 }
 435                 handle->mDepthCallback(static_cast<mv_source_h>(handle->mInternalSource),
 436                                                            static_cast<DepthTypePtr>(depthData.data), depthData.width, depthData.height,
 437                                                            handle->mDepthUserData);
 438 #ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
 439                 if (handle->mPointcloudCallback) {
 440                         mv_3d_pointcloud_s p { .type = MV_3D_POINTCLOUD_TYPE_PCD_BIN,
 441                                                                    .pointcloud = handle->GetPointcloudFromSource(*pInput, depthData) };
 442
 443                         handle->mPointcloudCallback(handle->mInternalSource, static_cast<mv_3d_pointcloud_h>(&p),
 444                                                                                 handle->mPointcloudUserData);
 445                 }
 446 #endif
 447                 delete[] static_cast<unsigned char *>(pInput->data);
 448                 delete[] static_cast<unsigned char *>(pInput->extraData);
 449                 mv_source_clear(handle->mInternalSource);
 450         }
 451
 452         return nullptr;
 453 }
 454
 455 int Mv3d::WritePointcloudFile(mv_3d_pointcloud_h pointcloud, mv_3d_pointcloud_type_e type, const char *fileName)
 456 {
 457 #ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
 458         auto s = static_cast<mv_3d_pointcloud_s *>(pointcloud);
 459         if (!s) {
 460                 LOGE("Pointcloud data is NULL");
 461                 return MEDIA_VISION_ERROR_INVALID_PARAMETER;
 462         }
 463
 464         if (access(mPointcloudOutputPath.c_str(), W_OK) != 0) {
 465                 if (errno == EACCES || errno == EPERM) {
 466                         utility::LogInfo("Fail to access path[%s]: Permission Denied", mPointcloudOutputPath.c_str());
 467                         return MEDIA_VISION_ERROR_PERMISSION_DENIED;
 468                 }
 469
 470                 utility::LogInfo("Fail to access path[%s]: Invalid Path", mPointcloudOutputPath.c_str());
 471                 return MEDIA_VISION_ERROR_INVALID_PARAMETER;
 472         }
 473
 474         bool bText = (type == MV_3D_POINTCLOUD_TYPE_PCD_TXT || type == MV_3D_POINTCLOUD_TYPE_PLY_TXT);
 475
 476         std::string fullPath = mPointcloudOutputPath + std::string("/") + std::string(fileName);
 477
 478         if (!io::WritePointCloud(fullPath.c_str(), *(s->pointcloud), { bText, false, false, {} })) {
 479                 utility::LogError("Failed to write {}", fullPath.c_str());
 480                 return MEDIA_VISION_ERROR_INTERNAL;
 481         }
 482
 483         utility::LogInfo("Successfully wrote {}", fullPath.c_str());
 484
 485         return MEDIA_VISION_ERROR_NONE;
 486 #else
 487         return MEDIA_VISION_ERROR_NOT_SUPPORTED;
 488 #endif
 489 }
 490 } // namespace mv3d
 491 } // namespace mediavision