From 849a2e27ef5018fe648df9a101c3b389826a7da7 Mon Sep 17 00:00:00 2001 From: sangho park Date: Wed, 21 Sep 2022 16:46:01 +0900 Subject: Add pointcloud implementation for mv_3d APIs [Issue type] new feature Change-Id: I7571bb1d67607e7d0c423296ce7d4c5c6fb7de6e Signed-off-by: sangho park --- include/mv_3d_internal.h | 103 ++++++ include/mv_3d_private.h | 12 +- media-vision-config.json | 15 +- mv_3d/3d/CMakeLists.txt | 10 +- mv_3d/3d/include/Depth.h | 93 ----- mv_3d/3d/include/Mv3d.h | 87 ++++- mv_3d/3d/include/PointCloud.h | 42 --- mv_3d/3d/include/mv_3d_open.h | 27 +- mv_3d/3d/src/Depth.cpp | 299 ---------------- mv_3d/3d/src/Mv3d.cpp | 443 ++++++++++++++++++++++++ mv_3d/3d/src/PointCloud.cpp | 28 -- mv_3d/3d/src/mv_3d internal.cpp | 154 ++++++++ mv_3d/3d/src/mv_3d.c | 4 +- mv_3d/3d/src/mv_3d_open.cpp | 114 +++++- test/testsuites/mv3d/CMakeLists.txt | 4 - test/testsuites/mv3d/depth_test_suite.cpp | 208 ++++------- test/testsuites/mv3d/depthstream_test_suite.cpp | 4 +- 17 files changed, 998 insertions(+), 649 deletions(-) create mode 100644 include/mv_3d_internal.h delete mode 100644 mv_3d/3d/include/Depth.h delete mode 100644 mv_3d/3d/include/PointCloud.h delete mode 100644 mv_3d/3d/src/Depth.cpp create mode 100644 mv_3d/3d/src/Mv3d.cpp delete mode 100644 mv_3d/3d/src/PointCloud.cpp create mode 100644 mv_3d/3d/src/mv_3d internal.cpp diff --git a/include/mv_3d_internal.h b/include/mv_3d_internal.h new file mode 100644 index 00000000..e499715f --- /dev/null +++ b/include/mv_3d_internal.h @@ -0,0 +1,103 @@ +/* + * Copyright (c) 2022 Samsung Electronics Co., Ltd All Rights Reserved + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef __TIZEN_MEDIAVISION_3D_INTERNAL_H__ +#define __TIZEN_MEDIAVISION_3D_INTERNAL_H__ + +#include "mv_3d.h" + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +/** + * @file mv_3d_internal.h + * @brief This file contains the mv3d internal structure and function for plane segmentation. + * @since_tizen 7.0 + */ + +/** + * @brief The plane model handle. + * @since_tizen 7.0 + */ +typedef void *mv_3d_pointcloud_plane_model_h; + +/** + * @brief The plane inlier handle. + * @since_tizen 7.0 + */ +typedef void *mv_3d_pointcloud_plane_inlier_h; + +/** + * @brief Creates plane model handle. + * @details Use this function to create a plane model handle. + * + * @since_tizen 7.0 + */ +int mv_3d_pointcloud_plane_model_create(mv_3d_pointcloud_plane_model_h *handle); + +/** + * @brief Destroys plane model handle and release all its resources. + * + * @since_tizen 7.0 + */ +int mv_3d_pointcloud_plane_model_destroy(mv_3d_pointcloud_plane_model_h handle); + +/** + * @brief Creates plane inlier handle. + * @details Use this function to create a plane model handle. + * + * @since_tizen 7.0 + */ +int mv_3d_pointcloud_plane_inlier_create(mv_3d_pointcloud_plane_inlier_h *handle); + +/** + * @brief Destroys plane inlier handle and release all its resources. + * + * @since_tizen 7.0 + */ +int mv_3d_pointcloud_plane_inlier_destroy(mv_3d_pointcloud_plane_inlier_h handle); + +/** + * @brief Segment PointCloud plane. + * @details Use this function to segment pointcloud plane using the RANSAC algorithm. + * + * @since_tizen 7.0 + */ +int mv_3d_pointcloud_segment_plane(mv_3d_h mv3d, + mv_3d_pointcloud_h pointcloud, + mv_3d_pointcloud_plane_model_h *plane_model, + mv_3d_pointcloud_plane_inlier_h *plane_inlier); + +/** + * @brief Writes pointcloud plane data to a file. + * @details Use this function to write pointcloud plane data to a file. + * + * @since_tizen 7.0 + */ + +int mv_3d_pointcloud_plane_write_file(mv_3d_h mv3d, + mv_3d_pointcloud_plane_model_h model, + mv_3d_pointcloud_plane_inlier_h inlier, + mv_3d_pointcloud_h pointcloud, + mv_3d_pointcloud_type_e type, + char *filename); + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* __TIZEN_MEDIAVISION_3D_INTERNAL_H__ */ diff --git a/include/mv_3d_private.h b/include/mv_3d_private.h index 84dcc982..37bcc0bb 100644 --- a/include/mv_3d_private.h +++ b/include/mv_3d_private.h @@ -31,15 +31,15 @@ extern "C" { */ /** - * @brief Landmarks of a pose. + * @brief structure of pointcloud. * - * @since_tizen 6.0 + * @since_tizen 7.0 * */ typedef struct { - mv_pointcloud_type_e type; - FILE *pointcloud; -} mv_pointcloud_s; + mv_3d_pointcloud_type_e type; + void *pointcloud; +} mv_3d_pointcloud_s; /** * @} @@ -48,4 +48,4 @@ typedef struct { } #endif /* __cplusplus */ -#endif /* __TIZEN_MEDIAVISION_3D_TYPE_H__ */ +#endif /* __TIZEN_MEDIAVISION_3D_PRIVATE_H__ */ diff --git a/media-vision-config.json b/media-vision-config.json index d48ff9ef..6703092c 100644 --- a/media-vision-config.json +++ b/media-vision-config.json @@ -284,19 +284,24 @@ "value" : "" }, { - "name" : "MV_3D_POINTCLOUD_SAMPLING_RATIO", + "name" : "MV_3D_POINTCLOUD_SAMPLING_RATIO", "type" : "double", "value" : 1.0 }, { - "name" : "MV_3D_POINTCLOUD_OUTLIER_REMOVAL_POINTS", + "name" : "MV_3D_POINTCLOUD_OUTLIER_REMOVAL_POINTS", "type" : "integer", - "value" : 10 + "value" : 0 }, { - "name" : "MV_3D_POINTCLOUD_OUTLIER_REMOVAL_RADIUS", + "name" : "MV_3D_POINTCLOUD_OUTLIER_REMOVAL_RADIUS", "type" : "double", - "value" : 1.0 + "value" : 0.0 + }, + { + "name" : "MV_3D_POINTCLOUD_OUTPUT_FILE_PATH", + "type" : "string", + "value" : "/opt/usr/home/owner/media/Images" } ] } diff --git a/mv_3d/3d/CMakeLists.txt b/mv_3d/3d/CMakeLists.txt index d2eb9af6..2265cbdb 100644 --- a/mv_3d/3d/CMakeLists.txt +++ b/mv_3d/3d/CMakeLists.txt @@ -10,12 +10,20 @@ if(NOT OpenCV_FOUND) return() endif() +find_package(Open3D REQUIRED NO_POLICY_SCOPE) +if(NOT Open3D_FOUND) + message(SEND_ERROR "Open3D NOT FOUND") + return() +else() + include_directories(${Open3D_INCLUDE_DIRS}) +endif() + if(FORCED_STATIC_BUILD) add_library(${PROJECT_NAME} STATIC ${MV_DFS_SOURCE_LIST}) else() add_library(${PROJECT_NAME} SHARED ${MV_DFS_SOURCE_LIST}) endif() -target_link_libraries(${PROJECT_NAME} ${MV_COMMON_LIB_NAME} ${OpenCV_LIBS} ${${PROJECT_NAME}_DEP_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${MV_COMMON_LIB_NAME} ${OpenCV_LIBS} ${Open3D_LIBRARIES} ${${PROJECT_NAME}_DEP_LIBRARIES}) target_include_directories(${PROJECT_NAME} PRIVATE include) install(TARGETS ${PROJECT_NAME} DESTINATION ${LIB_INSTALL_DIR}) diff --git a/mv_3d/3d/include/Depth.h b/mv_3d/3d/include/Depth.h deleted file mode 100644 index b3236c45..00000000 --- a/mv_3d/3d/include/Depth.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright (c) 2022 Samsung Electronics Co., Ltd All Rights Reserved - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef __MEDIA_VISION_DEPTH_H__ -#define __MEDIA_VISION_DEPTH_H__ - -#include -#include -#include "dfs_parameter.h" -#include "dfs_adaptation_impl.h" -#include "mv_3d.h" -#include "mv_3d_type.h" - -/** - * @file Depth.h - * @brief This file contains the depth class definition - * which supports depth-from-stereo (DFS). - */ -using namespace DfsAdaptation; -using DepthType = uint16_t; -using DepthTypePtr = DepthType*; -namespace mediavision -{ -namespace depth -{ - class Depth - { - private: - DfsParameter mDfsParameter; - DfsAdaptor *mDfsAdaptor; - int mMode; - - size_t mWidth; - size_t mHeight; - size_t mMinDisp; - size_t mMaxDisp; - std::string mStereoConfigPath; - - GThread *mThread; - void *mUserData; - mv_3d_depth_cb mDepthCallback; - bool mIsLive; - - GAsyncQueue *mAsyncQueue; - - void GetBufferFromSource(mv_source_h source, - unsigned char*& buffer, - unsigned int& width, - unsigned int& height, - int& type, - size_t& stride); - - void GetDfsDataFromSources(mv_source_h baseSource, - mv_source_h extraSource, - DfsInputData& input); - - - static gpointer ThreadLoop(gpointer data); - public: - Depth(); - ~Depth(); - void SetParameters(double threshold, - size_t windowWidth, - size_t windowHeight, - size_t speckleSize); - - int Configure(int mode, int width, int height, int minDisp, int maxDisp, - std::string stereoConfigPath); - - void SetCallback(mv_3d_depth_cb depthCallback, void *userData); - - int Prepare(); - - int Run(mv_source_h baseSource, mv_source_h extraSource); - - int RunAsync(mv_source_h baseSource, mv_source_h extraSource); - }; -} -} -#endif /* __MEDIA_VISION_DEPTH_H__ */ diff --git a/mv_3d/3d/include/Mv3d.h b/mv_3d/3d/include/Mv3d.h index dc8b36c1..8e345a80 100644 --- a/mv_3d/3d/include/Mv3d.h +++ b/mv_3d/3d/include/Mv3d.h @@ -17,30 +17,93 @@ #ifndef __MEDIA_VISION_3D_H__ #define __MEDIA_VISION_3D_H__ + #include -#include "Depth.h" -#include "PointCloud.h" +#include +#include "dfs_parameter.h" +#include "dfs_adaptation_impl.h" +#include "mv_3d.h" +#include "mv_3d_type.h" +#include "mv_3d_private.h" /** - * @file Mv3d.h - * @brief This file contains the mv3d class definition - * which supports depth and point cloud data. + * @file Depth.h + * @brief This file contains the depth class definition + * which supports depth-from-stereo (DFS). */ -using namespace mediavision::depth; -using namespace mediavision::pointcloud; +using namespace DfsAdaptation; +using DepthType = uint16_t; +using DepthTypePtr = DepthType*; namespace mediavision { namespace mv3d { class Mv3d { - public: - Depth mDepth; - PointCloud mPointCloud; + private: + DfsParameter mDfsParameter; + DfsAdaptor *mDfsAdaptor; + int mMode; + + size_t mWidth; + size_t mHeight; + size_t mMinDisp; + size_t mMaxDisp; + double mSamplingRatio; + int mOutlierRemovalPoints; + double mOutlierRemovalRadius; + std::string mStereoConfigPath; + std::string mIntrinsicPath; + std::string mPointcloudOutputPath; + + GThread *mDfsThread; + void *mDepthUserData; + void *mPointcloudUserData; + mv_3d_depth_cb mDepthCallback; + mv_3d_pointcloud_cb mPointcloudCallback; + bool mDfsIsLive; + + GAsyncQueue *mDfsAsyncQueue; + + void GetBufferFromSource(mv_source_h source, + unsigned char*& buffer, + unsigned int& width, + unsigned int& height, + int& type, + size_t& stride); + + void GetDfsDataFromSources(mv_source_h baseSource, + mv_source_h extraSource, + DfsInputData& input); + + static gpointer DfsThreadLoop(gpointer data); + void GetPointcloudFromSource(DfsInputData &intput, + DfsOutputData &depthData, + mv_3d_pointcloud_s &pointcloud); public: - Mv3d() = default; - ~Mv3d() = default; + Mv3d(); + ~Mv3d(); + void SetParameters(double threshold, + size_t windowWidth, + size_t windowHeight, + size_t speckleSize); + + int Configure(int mode, int width, int height, int minDisp, int maxDisp, + double samplingRatio, int outlierRemovalPoints, double outlierRemovalRadius, + std::string stereoConfigPath, std::string pointcloudOutputPath); + + void SetDepthCallback(mv_3d_depth_cb depthCallback, void *depthUserData); + + void SetPointcloudCallback(mv_3d_pointcloud_cb pointcloudCallback, void *pointcloudUserData); + + int WritePointcloudFile(mv_3d_pointcloud_h pointcloud, mv_3d_pointcloud_type_e type, char *fileName); + + int Prepare(); + + int Run(mv_source_h baseSource, mv_source_h extraSource); + + int RunAsync(mv_source_h baseSource, mv_source_h extraSource); }; } } diff --git a/mv_3d/3d/include/PointCloud.h b/mv_3d/3d/include/PointCloud.h deleted file mode 100644 index dcc91cbc..00000000 --- a/mv_3d/3d/include/PointCloud.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright (c) 2022 Samsung Electronics Co., Ltd All Rights Reserved - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef __MEDIA_VISION_POINT_CLOUD_H__ -#define __MEDIA_VISION_POINT_CLOUD_H__ - -#include -#include -#include "mv_3d.h" -#include "mv_3d_type.h" - -/** - * @file PointCloud.h - * @brief This file contains the PointCloud class definition - * which supports Point Cloud. - */ -namespace mediavision -{ -namespace pointcloud -{ - class PointCloud - { - public: - PointCloud() = default; - ~PointCloud() = default; - }; -} -} -#endif /* __MEDIA_VISION_DEPTH_H__ */ diff --git a/mv_3d/3d/include/mv_3d_open.h b/mv_3d/3d/include/mv_3d_open.h index b2c25824..00e8dd5e 100644 --- a/mv_3d/3d/include/mv_3d_open.h +++ b/mv_3d/3d/include/mv_3d_open.h @@ -49,6 +49,12 @@ int mv3dConfigure(mv_3d_h mv3d, mv_engine_config_h engine_config); */ int mv3dSetDepthCallback(mv_3d_h mv3d, mv_3d_depth_cb depth_cb, void *user_data); +/** + * @brief Set pointcloud callback to mv3d handle. + * @sicne_tizen 7.0 + */ +int mv3dSetPointcloudCallback(mv_3d_h mv3d, mv_3d_pointcloud_cb pointcloud_cb, void *user_data); + /** * @brief Prepares mv3d handle. * @since_tizen 7.0 @@ -59,17 +65,26 @@ int mv3dPrepare(mv_3d_h mv3d); * @brief Gets depth data from source(s). * @since_tizen 7.0 */ -int mv3dRun(mv_source_h source, - mv_source_h source_extra, - mv_3d_h mv3d); +int mv3dRun(mv_3d_h mv3d, + mv_source_h source, + mv_source_h source_extra); /** * @brief Run depth estimation asynchronousely with source(s). * @since_tizen 7.0 */ -int mv3dRunAsync(mv_source_h source, - mv_source_h source_extra, - mv_3d_h mv3d); +int mv3dRunAsync(mv_3d_h mv3d, + mv_source_h source, + mv_source_h source_extra); + +/** + * @brief Write Pointcloud file. + * @since_tizen 7.0 + */ +int mv3dWritePointcloudFile(mv_3d_h mv3d, + mv_3d_pointcloud_h pointcloud, + mv_3d_pointcloud_type_e type, + char *fileName); #ifdef __cplusplus } diff --git a/mv_3d/3d/src/Depth.cpp b/mv_3d/3d/src/Depth.cpp deleted file mode 100644 index 01e3197b..00000000 --- a/mv_3d/3d/src/Depth.cpp +++ /dev/null @@ -1,299 +0,0 @@ -/* - * Copyright (c) 2022 Samsung Electronics Co., Ltd All Rights Reserved - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "mv_private.h" -#include -#include -#include "Depth.h" - -namespace mediavision -{ -namespace depth -{ - Depth::Depth() : - mDfsParameter(), - mDfsAdaptor(nullptr), - mMode(MV_3D_DEPTH_MODE_NONE), - mWidth(0), - mHeight(0), - mMinDisp(0), - mMaxDisp(0), - mThread(nullptr), - mUserData(nullptr), - mDepthCallback(nullptr), - mIsLive(false), - mAsyncQueue(nullptr) - { - LOGI("ENTER"); - LOGI("LEAVE"); - } - - Depth::~Depth() - { - LOGI("ENTER"); - - if (mThread) { - mIsLive = false; - g_thread_join(mThread); - } - - if (mAsyncQueue) { - g_async_queue_unref(mAsyncQueue); - } - - if (mDfsAdaptor) { - mDfsAdaptor->unBind(); - delete mDfsAdaptor; - } - - LOGI("LEAVE"); - } - - void Depth::SetParameters(double threshold, - size_t windowWidth, - size_t windowHeight, - size_t speckleSize) - { - mDfsParameter.textureThreshold = threshold; - mDfsParameter.aggregationWindowWidth = windowWidth; - mDfsParameter.aggregationWindowHeight = windowHeight; - mDfsParameter.maxSpeckleSize = speckleSize; - } - - int Depth::Configure(int mode, int width, int height, - int minDisp, int maxDisp, std::string stereoConfigPath) - { - mMode = mode; - mWidth = width; - mHeight = height; - mMinDisp = minDisp; - mMaxDisp = maxDisp; - mStereoConfigPath = stereoConfigPath; - - try { - mDfsAdaptor = new DfsAdaptor(); - mDfsAdaptor->bind(); - } catch (const std::bad_alloc &e) { - LOGE("Failed to create dfs adaptation : %s", e.what()); - return MEDIA_VISION_ERROR_OUT_OF_MEMORY; - } catch (const std::runtime_error &e) { - LOGE("Failed to bind %s adpator", e.what()); - return MEDIA_VISION_ERROR_INVALID_OPERATION; - } - - return MEDIA_VISION_ERROR_NONE; - } - - void Depth::SetCallback(mv_3d_depth_cb depthCallback, void *userData) - { - mDepthCallback = depthCallback; - mUserData = userData; - } - - int Depth::Prepare() - { - if (!mDfsAdaptor) { - LOGE("Invalid Opertation. Do Configure first."); - return MEDIA_VISION_ERROR_INVALID_OPERATION; - } - - try { - mDfsAdaptor->initialize(mDfsParameter, mWidth, mHeight, - mMinDisp, mMaxDisp, mStereoConfigPath); - } catch (const std::exception& e) { - LOGE("Failed to initialize"); - return MEDIA_VISION_ERROR_INVALID_OPERATION; - } - - return MEDIA_VISION_ERROR_NONE; - } - - - void Depth::GetBufferFromSource(mv_source_h source, - unsigned char*& buffer, - unsigned int& width, - unsigned int& height, - int& type, - size_t& stride) - { - unsigned char* _buffer = nullptr; - unsigned int _bufferSize = 0; - unsigned int _width = 0; - unsigned int _height = 0; - mv_colorspace_e _colorSpace = MEDIA_VISION_COLORSPACE_INVALID; - - int ret = mv_source_get_buffer(source, &_buffer, &_bufferSize); - if (ret != MEDIA_VISION_ERROR_NONE) - throw std::runtime_error("invalid buffer pointer"); - - ret = mv_source_get_width(source, &_width); - if (ret != MEDIA_VISION_ERROR_NONE) - throw std::runtime_error("invalid width"); - - ret = mv_source_get_height(source, &_height); - if (ret != MEDIA_VISION_ERROR_NONE) - throw std::runtime_error("invalid height"); - - ret = mv_source_get_colorspace(source, &_colorSpace); - if (ret != MEDIA_VISION_ERROR_NONE) - throw std::runtime_error("invalid color space"); - - buffer = new unsigned char [_bufferSize]; - memcpy(buffer, _buffer, _bufferSize); - width = _width; - height = _height; - type = _colorSpace == MEDIA_VISION_COLORSPACE_RGB888 ? - DFS_DATA_TYPE_UINT8C3 : - DFS_DATA_TYPE_UINT8C1; - stride = _bufferSize / _height; - } - - void Depth::GetDfsDataFromSources(mv_source_h baseSource, - mv_source_h extraSource, - DfsInputData& input) - { - unsigned char* baseBuffer = nullptr; - unsigned char* extraBuffer = nullptr; - unsigned int width = 0; - unsigned int height = 0; - int type = 0; - size_t stride = 0; - - GetBufferFromSource(baseSource, baseBuffer, width, height, type, stride); - input.data = static_cast(baseBuffer); - input.type = type; - input.width = width; - input.height = height; - input.stride = stride; - input.format = DFS_DATA_INPUT_FORMAT_COUPLED_SBS; - - if (extraSource) - { - extraBuffer = nullptr; - GetBufferFromSource(extraSource, extraBuffer, width, height, type, stride); - - if (input.type != type || input.width != width || - input.height != height || input.stride != stride) { - throw std::runtime_error("left and right image's properties are different"); - } - - input.extraData = static_cast(extraBuffer); - input.format = DFS_DATA_INPUT_FORMAT_DECOUPLED_SBS; - } - } - - int Depth::Run(mv_source_h baseSource, mv_source_h extraSource) - { - DfsInputData input; - try { - if (mThread) { - mIsLive = false; - g_thread_join(mThread); - mThread = nullptr; - } - - if (mAsyncQueue) { - g_async_queue_unref(mAsyncQueue); - mAsyncQueue = nullptr; - } - - GetDfsDataFromSources(baseSource, extraSource, input); - - mDfsAdaptor->run(input); - - auto depthData = mDfsAdaptor->getOutputData(); - - mDepthCallback( - baseSource, - static_cast(depthData.data), - depthData.width, depthData.height, - mUserData); - - delete [] input.data; - delete [] input.extraData; - } catch (const std::exception &e) { - LOGE("Failed to Run with %s", e.what()); - return MEDIA_VISION_ERROR_INVALID_OPERATION; - } - - return MEDIA_VISION_ERROR_NONE; - } - - int Depth::RunAsync(mv_source_h baseSource, mv_source_h extraSource) - { - try { - if (!mAsyncQueue) { - mAsyncQueue = g_async_queue_new(); - if (!mAsyncQueue) { - LOGE("Fail to g_async_queue_new()"); - return MEDIA_VISION_ERROR_INTERNAL; - } - } - - if (!mThread) { - mThread = g_thread_new("depth_thread", &Depth::ThreadLoop, static_cast(this)); - - if (!mThread) { - g_async_queue_unref(mAsyncQueue); - mAsyncQueue = nullptr; - LOGE("Fail to g_thread_new()"); - return MEDIA_VISION_ERROR_INTERNAL; - } - mIsLive = true; - } - - std::shared_ptr input(new DfsInputData); - GetDfsDataFromSources(baseSource, extraSource, *input); - g_async_queue_push(mAsyncQueue, static_cast( - new std::shared_ptr( - std::move(input)))); - } catch (const std::exception &e) { - LOGE("Failed to Run with %s", e.what()); - return MEDIA_VISION_ERROR_INVALID_OPERATION; - } - - return MEDIA_VISION_ERROR_NONE; - } - - gpointer Depth::ThreadLoop(gpointer data) - { - Depth *handle = static_cast(data); - while(handle->mIsLive) { - gpointer base = g_async_queue_try_pop(handle->mAsyncQueue); - if (!base) { - continue; - } - - auto pInput = static_cast*>(base); - auto input = std::move(*pInput); - delete pInput; - handle->mDfsAdaptor->run(*input); - delete [] static_cast(input->data); - delete [] static_cast(input->extraData); - input.reset(); - - auto depthData = handle->mDfsAdaptor->getOutputData(); - handle->mDepthCallback( - static_cast(base), - static_cast(depthData.data), - depthData.width, depthData.height, - handle->mUserData); - } - - return nullptr; - } -} -} diff --git a/mv_3d/3d/src/Mv3d.cpp b/mv_3d/3d/src/Mv3d.cpp new file mode 100644 index 00000000..747a786d --- /dev/null +++ b/mv_3d/3d/src/Mv3d.cpp @@ -0,0 +1,443 @@ +/* + * Copyright (c) 2022 Samsung Electronics Co., Ltd All Rights Reserved + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "mv_private.h" +#include +#include +#include +#include +#include +#include +#include + +#include "Mv3d.h" + +using namespace open3d; + +namespace mediavision +{ +namespace mv3d +{ + Mv3d::Mv3d() : + mDfsParameter(), + mDfsAdaptor(nullptr), + mMode(MV_3D_DEPTH_MODE_NONE), + mWidth(0), + mHeight(0), + mMinDisp(0), + mMaxDisp(0), + mSamplingRatio(1.0), + mOutlierRemovalPoints(0), + mOutlierRemovalRadius(0.0), + mDfsThread(nullptr), + mDepthUserData(nullptr), + mPointcloudUserData(nullptr), + mDepthCallback(nullptr), + mPointcloudCallback(nullptr), + mDfsIsLive(false), + mDfsAsyncQueue(nullptr) + { + LOGI("ENTER"); + utility::SetVerbosityLevel(utility::VerbosityLevel::Debug); + LOGI("LEAVE"); + } + + Mv3d::~Mv3d() + { + LOGI("ENTER"); + + if (mDfsThread) { + mDfsIsLive = false; + g_thread_join(mDfsThread); + } + + if (mDfsAsyncQueue) { + g_async_queue_unref(mDfsAsyncQueue); + } + + if (mDfsAdaptor) { + mDfsAdaptor->unBind(); + delete mDfsAdaptor; + } + + LOGI("LEAVE"); + } + + void Mv3d::SetParameters(double threshold, + size_t windowWidth, + size_t windowHeight, + size_t speckleSize) + { + mDfsParameter.textureThreshold = threshold; + mDfsParameter.aggregationWindowWidth = windowWidth; + mDfsParameter.aggregationWindowHeight = windowHeight; + mDfsParameter.maxSpeckleSize = speckleSize; + } + + int Mv3d::Configure(int mode, int width, int height, + int minDisp, int maxDisp, double samplingRatio, + int outlierRemovalPoints, double outlierRemovalRadius, + std::string stereoConfigPath, + std::string pointcloudOutputPath) + { + mMode = mode; + mWidth = width; + mHeight = height; + mMinDisp = minDisp; + mMaxDisp = maxDisp; + mSamplingRatio = samplingRatio; + mOutlierRemovalPoints = outlierRemovalPoints; + mOutlierRemovalRadius = outlierRemovalRadius; + mStereoConfigPath = stereoConfigPath; + size_t found = stereoConfigPath.rfind("."); + mIntrinsicPath = stereoConfigPath.substr(0, found) + std::string(".json"); + mPointcloudOutputPath = pointcloudOutputPath; + + try { + mDfsAdaptor = new DfsAdaptor(); + mDfsAdaptor->bind(); + } catch (const std::bad_alloc &e) { + LOGE("Failed to create dfs adaptation : %s", e.what()); + return MEDIA_VISION_ERROR_OUT_OF_MEMORY; + } catch (const std::runtime_error &e) { + LOGE("Failed to bind %s adpator", e.what()); + return MEDIA_VISION_ERROR_INVALID_OPERATION; + } + + return MEDIA_VISION_ERROR_NONE; + } + + void Mv3d::SetDepthCallback(mv_3d_depth_cb depthCallback, void *depthUserData) + { + mDepthCallback = depthCallback; + mDepthUserData = depthUserData; + } + + void Mv3d::SetPointcloudCallback(mv_3d_pointcloud_cb pointcloudCallback, void *pointcloudUserData) + { + mPointcloudCallback = pointcloudCallback; + mPointcloudUserData = pointcloudUserData; + } + + int Mv3d::Prepare() + { + if (!mDfsAdaptor) { + LOGE("Invalid Opertation. Do Configure first."); + return MEDIA_VISION_ERROR_INVALID_OPERATION; + } + + try { + mDfsAdaptor->initialize(mDfsParameter, mWidth, mHeight, + mMinDisp, mMaxDisp, mStereoConfigPath); + } catch (const std::exception& e) { + LOGE("Failed to initialize"); + return MEDIA_VISION_ERROR_INVALID_OPERATION; + } + + return MEDIA_VISION_ERROR_NONE; + } + + + void Mv3d::GetBufferFromSource(mv_source_h source, + unsigned char*& buffer, + unsigned int& width, + unsigned int& height, + int& type, + size_t& stride) + { + unsigned char* _buffer = nullptr; + unsigned int _bufferSize = 0; + unsigned int _width = 0; + unsigned int _height = 0; + mv_colorspace_e _colorSpace = MEDIA_VISION_COLORSPACE_INVALID; + + int ret = mv_source_get_buffer(source, &_buffer, &_bufferSize); + if (ret != MEDIA_VISION_ERROR_NONE) + throw std::runtime_error("invalid buffer pointer"); + + ret = mv_source_get_width(source, &_width); + if (ret != MEDIA_VISION_ERROR_NONE) + throw std::runtime_error("invalid width"); + + ret = mv_source_get_height(source, &_height); + if (ret != MEDIA_VISION_ERROR_NONE) + throw std::runtime_error("invalid height"); + + ret = mv_source_get_colorspace(source, &_colorSpace); + if (ret != MEDIA_VISION_ERROR_NONE) + throw std::runtime_error("invalid color space"); + + buffer = new unsigned char [_bufferSize]; + memcpy(buffer, _buffer, _bufferSize); + width = _width; + height = _height; + type = _colorSpace == MEDIA_VISION_COLORSPACE_RGB888 ? + DFS_DATA_TYPE_UINT8C3 : + DFS_DATA_TYPE_UINT8C1; + stride = _bufferSize / _height; + } + + void Mv3d::GetDfsDataFromSources(mv_source_h baseSource, + mv_source_h extraSource, + DfsInputData& input) + { + unsigned char* baseBuffer = nullptr; + unsigned char* extraBuffer = nullptr; + unsigned int width = 0; + unsigned int height = 0; + int type = 0; + size_t stride = 0; + + GetBufferFromSource(baseSource, baseBuffer, width, height, type, stride); + input.data = static_cast(baseBuffer); + input.type = type; + input.width = width; + input.height = height; + input.stride = stride; + input.format = DFS_DATA_INPUT_FORMAT_COUPLED_SBS; + + if (extraSource) + { + extraBuffer = nullptr; + GetBufferFromSource(extraSource, extraBuffer, width, height, type, stride); + + if (input.type != type || input.width != width || + input.height != height || input.stride != stride) { + throw std::runtime_error("left and right image's properties are different"); + } + + input.extraData = static_cast(extraBuffer); + input.format = DFS_DATA_INPUT_FORMAT_DECOUPLED_SBS; + } + } + + void Mv3d::GetPointcloudFromSource(DfsInputData &input, + DfsOutputData &depthData, + mv_3d_pointcloud_s &pointcloud) + { + camera::PinholeCameraIntrinsic intrinsic; + io::ReadIJsonConvertible(mIntrinsicPath, intrinsic); + cv::Mat img; + double depth_scale = 1000.0, depth_trunc = 200.0; + if (input.type == DFS_DATA_TYPE_UINT8C1) { + img = cv::Mat(cv::Size(input.width, input.height), CV_8UC1, input.data); + cv::cvtColor(img, img, cv::COLOR_GRAY2RGB); + } else if (input.type == DFS_DATA_TYPE_UINT8C3) { + img = cv::Mat(cv::Size(input.width, input.height), CV_8UC3, input.data); + cv::cvtColor(img, img, cv::COLOR_BGR2RGB); + } + + geometry::Image img_color, img_depth; + img_color.Prepare(input.width, input.height, 3, 1); + uint8_t *pImg_color_data = img_color.data_.data(); + memcpy(pImg_color_data, img.data, input.width * input.height * 3 * sizeof(unsigned char)); + + img_depth.Prepare(input.width, input.height, 1, 2); + uint16_t *pImg_depth_data = (unsigned short*)img_depth.data_.data(); + memcpy(pImg_depth_data, static_cast(depthData.data), depthData.width * depthData.height * 2); + + std::shared_ptr rgbd_image = + geometry::RGBDImage::CreateFromColorAndDepth( + img_color, img_depth, depth_scale, depth_trunc, false); + + auto pcd = geometry::PointCloud::CreateFromRGBDImage(*rgbd_image, intrinsic); + if (mSamplingRatio < 1.0) { + utility::LogInfo("Downsampling... {}", mSamplingRatio); + pcd = pcd->RandomDownSample(mSamplingRatio); + } + + if (mOutlierRemovalPoints > 0 && mOutlierRemovalRadius > 0.0) { + utility::LogInfo("RemoveRadiusOutliers... {} {}", mOutlierRemovalPoints, mOutlierRemovalRadius); + std::vector pt_map; + std::tie(pcd, pt_map) = pcd->RemoveRadiusOutliers(mOutlierRemovalPoints, mOutlierRemovalRadius, true); + } + + pointcloud.pointcloud = static_cast( + new std::shared_ptr( + std::move(pcd))); + + } + + int Mv3d::Run(mv_source_h baseSource, mv_source_h extraSource) + { + DfsInputData input; + try { + if (mDfsThread) { + mDfsIsLive = false; + g_thread_join(mDfsThread); + mDfsThread = nullptr; + } + + if (mDfsAsyncQueue) { + g_async_queue_unref(mDfsAsyncQueue); + mDfsAsyncQueue = nullptr; + } + + GetDfsDataFromSources(baseSource, extraSource, input); + + mDfsAdaptor->run(input); + auto depthData = mDfsAdaptor->getOutputData(); + + mDepthCallback( + baseSource, + static_cast(depthData.data), + depthData.width, depthData.height, + mDepthUserData); + + if (mPointcloudCallback) { + mv_3d_pointcloud_s p = {.type = MV_3D_POINTCLOUD_TYPE_PCD_BIN, .pointcloud = NULL}; + GetPointcloudFromSource(input, depthData, p); + + mv_3d_pointcloud_h pcd = &p; + mPointcloudCallback(baseSource, + pcd, + mPointcloudUserData); + auto pPcd = static_cast*>(p.pointcloud); + auto _pcd = std::move(pPcd); + delete _pcd; + } + delete [] static_cast(input.data); + delete [] static_cast(input.extraData); + } catch (const std::exception &e) { + LOGE("Failed to Run with %s", e.what()); + return MEDIA_VISION_ERROR_INVALID_OPERATION; + } + + return MEDIA_VISION_ERROR_NONE; + } + + int Mv3d::RunAsync(mv_source_h baseSource, mv_source_h extraSource) + { + try { + if (!mDfsAsyncQueue) { + mDfsAsyncQueue = g_async_queue_new(); + if (!mDfsAsyncQueue) { + LOGE("Fail to g_async_queue_new()"); + return MEDIA_VISION_ERROR_INTERNAL; + } + } + + if (!mDfsThread) { + mDfsThread = g_thread_new("depth_thread", &Mv3d::DfsThreadLoop, static_cast(this)); + + if (!mDfsThread) { + g_async_queue_unref(mDfsAsyncQueue); + mDfsAsyncQueue = nullptr; + LOGE("Fail to g_thread_new()"); + return MEDIA_VISION_ERROR_INTERNAL; + } + mDfsIsLive = true; + } + + std::shared_ptr input(new DfsInputData); + GetDfsDataFromSources(baseSource, extraSource, *input); + g_async_queue_push(mDfsAsyncQueue, static_cast( + new std::shared_ptr( + std::move(input)))); + } catch (const std::exception &e) { + LOGE("Failed to Run with %s", e.what()); + return MEDIA_VISION_ERROR_INVALID_OPERATION; + } + + return MEDIA_VISION_ERROR_NONE; + } + + gpointer Mv3d::DfsThreadLoop(gpointer data) + { + Mv3d *handle = static_cast(data); + while(handle->mDfsIsLive) { + gpointer base = g_async_queue_try_pop(handle->mDfsAsyncQueue); + if (!base) { + continue; + } + + auto pInput = static_cast*>(base); + auto input = std::move(*pInput); + delete pInput; + handle->mDfsAdaptor->run(*input); + + auto depthData = handle->mDfsAdaptor->getOutputData(); + handle->mDepthCallback( + static_cast(base), + static_cast(depthData.data), + depthData.width, depthData.height, + handle->mDepthUserData); + + if (handle->mPointcloudCallback) { + mv_3d_pointcloud_s p = {.type = MV_3D_POINTCLOUD_TYPE_PCD_BIN, .pointcloud = NULL}; + //mPointcloudThread = g_thread_new("pointcloud_thread", + // &Mv3d::PointcloudThreadLoop, + // static_cast(this)); + handle->GetPointcloudFromSource(*input, depthData, p); + + mv_3d_pointcloud_h pcd = &p; + handle->mPointcloudCallback(static_cast(base), + pcd, + handle->mPointcloudUserData); + auto pPcd = static_cast*>(p.pointcloud); + auto _pcd = std::move(pPcd); + delete _pcd; + } + + delete [] static_cast(input->data); + delete [] static_cast(input->extraData); + input.reset(); + } + + return nullptr; + } + + int Mv3d::WritePointcloudFile(mv_3d_pointcloud_h pointcloud, mv_3d_pointcloud_type_e type, char *fileName) + { + mv_3d_pointcloud_s *s = (mv_3d_pointcloud_s*) pointcloud; + if (s == NULL) { + LOGE("Pointcloud data is NULL"); + return MEDIA_VISION_ERROR_INVALID_PARAMETER; + } + + if (access(mPointcloudOutputPath.c_str(), W_OK) != 0) { + if (errno == EACCES || errno == EPERM) { + utility::LogInfo("Fail to access path[%s]: Permission Denied", mPointcloudOutputPath.c_str()); + return MEDIA_VISION_ERROR_PERMISSION_DENIED;; + } else { + utility::LogInfo("Fail to access path[%s]: Invalid Path", mPointcloudOutputPath.c_str()); + return MEDIA_VISION_ERROR_INVALID_PARAMETER; + } + } + + bool bText = false; + if (type == MV_3D_POINTCLOUD_TYPE_PCD_TXT || type == MV_3D_POINTCLOUD_TYPE_PLY_TXT) + bText = true; + else + bText = false; + + auto pPcd = static_cast*>(s->pointcloud); + open3d::geometry::PointCloud p; + p += **pPcd; + + std::string fullPath = mPointcloudOutputPath + std::string("/") + std::string(fileName); + + if (io::WritePointCloud(fullPath.c_str(), p, {bText, false, false, {}})) { + utility::LogInfo("Successfully wrote {}", fullPath.c_str()); + } else { + utility::LogError("Failed to write {}", fullPath.c_str()); + return MEDIA_VISION_ERROR_INTERNAL; + } + + return MEDIA_VISION_ERROR_NONE; + } +} +} diff --git a/mv_3d/3d/src/PointCloud.cpp b/mv_3d/3d/src/PointCloud.cpp deleted file mode 100644 index 083764d0..00000000 --- a/mv_3d/3d/src/PointCloud.cpp +++ /dev/null @@ -1,28 +0,0 @@ -/* - * Copyright (c) 2022 Samsung Electronics Co., Ltd All Rights Reserved - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "mv_private.h" -#include -#include -#include "PointCloud.h" - -namespace mediavision -{ -namespace pointCloud -{ -// -} -} diff --git a/mv_3d/3d/src/mv_3d internal.cpp b/mv_3d/3d/src/mv_3d internal.cpp new file mode 100644 index 00000000..4c52156a --- /dev/null +++ b/mv_3d/3d/src/mv_3d internal.cpp @@ -0,0 +1,154 @@ +/** + * Copyright (c) 2022 Samsung Electronics Co., Ltd All Rights Reserved + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "mv_3d_internal.h" +#include "mv_private.h" +#include "mv_3d_private.h" +#include "Mv3d.h" + +#define DISTANCE_THRESHOLD 0.01 +#define RANSAC_NUMBER 3 +#define NUM_ITERATIONS 1000 + +using namespace open3d; +using namespace mediavision::mv3d; + +int mv_3d_pointcloud_plane_model_create(mv_3d_pointcloud_plane_model_h *handle) +{ + MEDIA_VISION_NULL_ARG_CHECK(handle); + + MEDIA_VISION_FUNCTION_ENTER(); + + /* do nothing */ + + MEDIA_VISION_FUNCTION_LEAVE(); + + return MEDIA_VISION_ERROR_NONE; +} + +int mv_3d_pointcloud_plane_model_destroy(mv_3d_pointcloud_plane_model_h handle) +{ + MEDIA_VISION_INSTANCE_CHECK(handle); + + MEDIA_VISION_FUNCTION_ENTER(); + + Eigen::Vector4d *model = (Eigen::Vector4d *) handle; + delete model; + + MEDIA_VISION_FUNCTION_LEAVE(); + + return MEDIA_VISION_ERROR_NONE; +} + +int mv_3d_pointcloud_plane_inlier_create(mv_3d_pointcloud_plane_inlier_h *handle) +{ + MEDIA_VISION_NULL_ARG_CHECK(handle); + + MEDIA_VISION_FUNCTION_ENTER(); + + /* do nothing */ + + MEDIA_VISION_FUNCTION_LEAVE(); + + return MEDIA_VISION_ERROR_NONE; +} + +int mv_3d_pointcloud_plane_inlier_destroy(mv_3d_pointcloud_plane_inlier_h handle) +{ + MEDIA_VISION_INSTANCE_CHECK(handle); + + MEDIA_VISION_FUNCTION_ENTER(); + + std::vector *inlier = (std::vector *) handle; + delete inlier; + + MEDIA_VISION_FUNCTION_LEAVE(); + + return MEDIA_VISION_ERROR_NONE; +} + +/** + * @file mv_3d_internal.c + * @brief This file contains Media Vision 3D internal module. + */ + +int mv_3d_pointcloud_segment_plane(mv_3d_h mv3d, + mv_3d_pointcloud_h pointcloud, + mv_3d_pointcloud_plane_model_h *model, + mv_3d_pointcloud_plane_inlier_h *inlier) +{ + MEDIA_VISION_NULL_ARG_CHECK(mv3d); + MEDIA_VISION_NULL_ARG_CHECK(pointcloud); + MEDIA_VISION_NULL_ARG_CHECK(model); + MEDIA_VISION_NULL_ARG_CHECK(inlier); + MEDIA_VISION_FUNCTION_ENTER(); + + mv_3d_pointcloud_s *s = (mv_3d_pointcloud_s*) pointcloud; + Eigen::Vector4d *best_plane_model = new Eigen::Vector4d; + std::vector *plane_inlier = new std::vector; + auto pPcd = static_cast*>(s->pointcloud); + open3d::geometry::PointCloud p; + p += **pPcd; + + std::tie(*best_plane_model, *plane_inlier) = p.SegmentPlane(DISTANCE_THRESHOLD, RANSAC_NUMBER, NUM_ITERATIONS); + *model = (mv_3d_pointcloud_plane_model_h) best_plane_model; + *inlier = (mv_3d_pointcloud_plane_inlier_h) plane_inlier; + + MEDIA_VISION_FUNCTION_LEAVE(); + + return MEDIA_VISION_ERROR_NONE; +} + +int mv_3d_pointcloud_plane_write_file(mv_3d_h mv3d, + mv_3d_pointcloud_plane_model_h model, + mv_3d_pointcloud_plane_inlier_h inlier, + mv_3d_pointcloud_h pointcloud, + mv_3d_pointcloud_type_e type, + char *filename) +{ + MEDIA_VISION_NULL_ARG_CHECK(mv3d); + MEDIA_VISION_NULL_ARG_CHECK(model); + MEDIA_VISION_NULL_ARG_CHECK(inlier); + MEDIA_VISION_FUNCTION_ENTER(); + + mv_3d_pointcloud_s *s = (mv_3d_pointcloud_s*) pointcloud; + Eigen::Vector4d *best_plane_model = (Eigen::Vector4d*) model; + std::vector *plane_inlier = (std::vector*) inlier; + auto pPcd = static_cast*>(s->pointcloud); + open3d::geometry::PointCloud p; + p += **pPcd; + + std::shared_ptr plane = p.SelectByIndex(*plane_inlier); + mv_3d_pointcloud_s _pcd = {.type = MV_3D_POINTCLOUD_TYPE_PCD_BIN, .pointcloud = NULL}; + _pcd.pointcloud = static_cast( + new std::shared_ptr( + std::move(plane))); + mv_3d_pointcloud_h pcd = &_pcd; + + auto pMv3d = static_cast(mv3d); + int ret = pMv3d->WritePointcloudFile(pcd, type, filename); + if (ret != MEDIA_VISION_ERROR_NONE) { + LOGE("Failed to write pointcloud plane file"); + ret = MEDIA_VISION_ERROR_INTERNAL; + } + + MEDIA_VISION_FUNCTION_LEAVE(); + + return ret; +} \ No newline at end of file diff --git a/mv_3d/3d/src/mv_3d.c b/mv_3d/3d/src/mv_3d.c index d766ed0e..c640e13f 100644 --- a/mv_3d/3d/src/mv_3d.c +++ b/mv_3d/3d/src/mv_3d.c @@ -92,7 +92,7 @@ int mv_3d_set_pointcloud_cb(mv_3d_h mv3d, MEDIA_VISION_FUNCTION_ENTER(); - int ret = MEDIA_VISION_ERROR_NONE; + int ret = mv3dSetPointcloudCallback(mv3d, pointcloud_cb, user_data); MEDIA_VISION_FUNCTION_LEAVE(); @@ -160,7 +160,7 @@ int mv_3d_pointcloud_write_file(mv_3d_h mv3d, MEDIA_VISION_FUNCTION_ENTER(); - int ret = MEDIA_VISION_ERROR_NONE; + int ret = mv3dWritePointcloudFile(mv3d, pointcloud, type, filename); MEDIA_VISION_FUNCTION_LEAVE(); diff --git a/mv_3d/3d/src/mv_3d_open.cpp b/mv_3d/3d/src/mv_3d_open.cpp index 7fd90268..9f914d24 100644 --- a/mv_3d/3d/src/mv_3d_open.cpp +++ b/mv_3d/3d/src/mv_3d_open.cpp @@ -24,7 +24,6 @@ #include "mv_3d_open.h" #include "Mv3d.h" -//using namespace mediavision::depth; using namespace mediavision::mv3d; int mv3dCreate(mv_3d_h *mv3d) @@ -67,7 +66,7 @@ int mv3dSetDepthParameters(mv_3d_h mv3d, mv_engine_config_h engine_config) auto pMv3d = static_cast(mv3d); - pMv3d->mDepth.SetParameters(127.5, // threshold + pMv3d->SetParameters(127.5, // threshold 3, // aggregation window width 3, // aggregation window height 0 // speckleSize @@ -142,15 +141,57 @@ int mv3dConfigure(mv_3d_h mv3d, mv_engine_config_h engine_config) return ret; } + double samplingRatio; + ret = mv_engine_config_get_double_attribute( + engine_config, MV_3D_POINTCLOUD_SAMPLING_RATIO,&samplingRatio); + if (ret != MEDIA_VISION_ERROR_NONE) { + LOGE("Failed to get stereo configuration file path"); + return ret; + } + + int outlierRemovalPoints; + ret = mv_engine_config_get_int_attribute( + engine_config, MV_3D_POINTCLOUD_OUTLIER_REMOVAL_POINTS, &outlierRemovalPoints); + if (ret != MEDIA_VISION_ERROR_NONE) { + LOGE("Failed to get stereo configuration file path"); + return ret; + } + + double outlierRemovalRadius; + ret = mv_engine_config_get_double_attribute( + engine_config, MV_3D_POINTCLOUD_OUTLIER_REMOVAL_RADIUS, &outlierRemovalRadius); + if (ret != MEDIA_VISION_ERROR_NONE) { + LOGE("Failed to get stereo configuration file path"); + return ret; + } + + char *pointcloudOutputFilePath = NULL; + ret = mv_engine_config_get_string_attribute( + engine_config, MV_3D_POINTCLOUD_OUTPUT_FILE_PATH, &pointcloudOutputFilePath); + if (ret != MEDIA_VISION_ERROR_NONE) { + LOGE("Failed to get stereo configuration file path"); + if (pointcloudOutputFilePath) { + free(pointcloudOutputFilePath); + pointcloudOutputFilePath = NULL; + } + return ret; + } + auto pMv3d = static_cast(mv3d); - ret = pMv3d->mDepth.Configure(mode, depthWidth, depthHeight, minDisp, maxDisp, - stereoConfigFilePath); + ret = pMv3d->Configure(mode, depthWidth, depthHeight, minDisp, maxDisp, + samplingRatio, outlierRemovalPoints, outlierRemovalRadius, + stereoConfigFilePath, pointcloudOutputFilePath); if (stereoConfigFilePath) { free(stereoConfigFilePath); stereoConfigFilePath = NULL; } + if (pointcloudOutputFilePath) { + free(pointcloudOutputFilePath); + pointcloudOutputFilePath = NULL; + } + if (ret != MEDIA_VISION_ERROR_NONE) { LOGE("Failed to configure Depth"); return ret; @@ -176,13 +217,35 @@ int mv3dSetDepthCallback(mv_3d_h mv3d, mv_3d_depth_cb depth_cb, void *user_data) } auto pMv3d = static_cast(mv3d); - pMv3d->mDepth.SetCallback(depth_cb, user_data); + pMv3d->SetDepthCallback(depth_cb, user_data); LOGI("LEAVE"); return MEDIA_VISION_ERROR_NONE; } + +int mv3dSetPointcloudCallback(mv_3d_h mv3d, mv_3d_pointcloud_cb pointcloud_cb, void *user_data) +{ + LOGI("ENTER"); + + if (!mv3d) { + LOGE("Handle is NULL"); + return MEDIA_VISION_ERROR_INVALID_PARAMETER; + } + + if (!pointcloud_cb) { + LOGE("Callbakc is NULL"); + return MEDIA_VISION_ERROR_INVALID_PARAMETER; + } + + auto pMv3d = static_cast(mv3d); + pMv3d->SetPointcloudCallback(pointcloud_cb, user_data); + + LOGI("LEAVE"); + return MEDIA_VISION_ERROR_NONE; +} + int mv3dPrepare(mv_3d_h mv3d) { LOGI("ENTER"); @@ -193,7 +256,7 @@ int mv3dPrepare(mv_3d_h mv3d) } auto pMv3d = static_cast(mv3d); - int ret = pMv3d->mDepth.Prepare(); + int ret = pMv3d->Prepare(); if (ret != MEDIA_VISION_ERROR_NONE) { LOGE("Failed to prepare depth"); return ret; @@ -204,9 +267,9 @@ int mv3dPrepare(mv_3d_h mv3d) return ret; } -int mv3dRun(mv_source_h source, - mv_source_h source_extra, - mv_3d_h mv3d) +int mv3dRun(mv_3d_h mv3d, + mv_source_h source, + mv_source_h source_extra) { LOGI("ENTER"); @@ -218,7 +281,7 @@ int mv3dRun(mv_source_h source, auto pMv3d = static_cast(mv3d); - int ret = pMv3d->mDepth.Run(source, source_extra); + int ret = pMv3d->Run(source, source_extra); if (ret != MEDIA_VISION_ERROR_NONE) { LOGE("Failed to run depth"); return ret; @@ -229,9 +292,9 @@ int mv3dRun(mv_source_h source, return ret; } -int mv3dRunAsync(mv_source_h source, - mv_source_h source_extra, - mv_3d_h mv3d) +int mv3dRunAsync(mv_3d_h mv3d, + mv_source_h source, + mv_source_h source_extra) { LOGI("ENTER"); @@ -242,7 +305,7 @@ int mv3dRunAsync(mv_source_h source, } auto pMv3d = static_cast(mv3d); - int ret = pMv3d->mDepth.RunAsync(source, source_extra); + int ret = pMv3d->RunAsync(source, source_extra); if (ret != MEDIA_VISION_ERROR_NONE) { LOGE("Failed to run depth"); return ret; @@ -251,4 +314,25 @@ int mv3dRunAsync(mv_source_h source, LOGI("LEAVE"); return ret; -} \ No newline at end of file +} + +int mv3dWritePointcloudFile(mv_3d_h mv3d, mv_3d_pointcloud_h pointcloud, mv_3d_pointcloud_type_e type, char *fileName) +{ + LOGI("ENTER"); + + if (!mv3d) { + LOGE("Handle is NULL"); + return MEDIA_VISION_ERROR_INVALID_PARAMETER; + } + + auto pMv3d = static_cast(mv3d); + int ret = pMv3d->WritePointcloudFile(pointcloud, type, fileName); + if (ret != MEDIA_VISION_ERROR_NONE) { + LOGE("Failed to write pointcloud file"); + return ret; + } + + LOGI("LEAVE"); + + return ret; +} diff --git a/test/testsuites/mv3d/CMakeLists.txt b/test/testsuites/mv3d/CMakeLists.txt index 38abf666..8509a3d6 100644 --- a/test/testsuites/mv3d/CMakeLists.txt +++ b/test/testsuites/mv3d/CMakeLists.txt @@ -18,7 +18,6 @@ endif() SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-format-truncation") add_executable(${PROJECT_NAME} depth_test_suite.cpp) target_link_libraries(${PROJECT_NAME} ${MV_3D_LIB_NAME} - ${Open3D_LIBRARIES} ${OpenCV_LIBS} mv_image_helper mv_video_helper @@ -26,7 +25,6 @@ target_link_libraries(${PROJECT_NAME} ${MV_3D_LIB_NAME} if(BUILD_VISUALIZER) target_link_libraries(${PROJECT_NAME} mv_visualizer) endif() -target_include_directories(${PROJECT_NAME} PUBLIC ${Open3D_INCLUDE_DIRS}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_BINDIR}) ## mv_depthstream @@ -54,7 +52,6 @@ SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") add_executable(mv_depthstream_test_suite depthstream_test_suite.cpp) target_link_libraries(mv_depthstream_test_suite ${MV_3D_LIB_NAME} - ${Open3D_LIBRARIES} ${OpenCV_LIBS} gstreamer-1.0 glib-2.0 @@ -67,5 +64,4 @@ if(BUILD_VISUALIZER) target_link_libraries(mv_depthstream_test_suite mv_visualizer) endif() -target_include_directories(mv_depthstream_test_suite PUBLIC ${Open3D_INCLUDE_DIRS}) install(TARGETS mv_depthstream_test_suite DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/test/testsuites/mv3d/depth_test_suite.cpp b/test/testsuites/mv3d/depth_test_suite.cpp index 71bbe3d6..e062039a 100644 --- a/test/testsuites/mv3d/depth_test_suite.cpp +++ b/test/testsuites/mv3d/depth_test_suite.cpp @@ -26,14 +26,15 @@ #include #include #include -#include #include #include #include #include "mv_3d.h" +#include "mv_3d_internal.h" +#define MEDIA_FILE_PATH "/opt/usr/home/owner/media/Images" #ifdef BUILD_VISUALIZER #include "mv_util_visualizer_3d.h" #endif @@ -44,8 +45,6 @@ #define __max(a,b) (((a) > (b)) ? (a) : (b)) #define __min(a,b) (((a) < (b)) ? (a) : (b)) -using namespace open3d; - /* There are calib.txt, im0.png, and im1.png in each dataset directories.*/ static const char* dataset[] = { "Adirondack", @@ -90,6 +89,7 @@ private: }; typedef struct _appdata { + mv_3d_h mv3d; std::chrono::milliseconds diffMs; std::string dataPath; std::string intrinsicName; @@ -98,6 +98,7 @@ typedef struct _appdata { float minDisp; float maxDisp; int fmt; + std::string dataset; } appdata; enum { @@ -201,111 +202,6 @@ void WritePLY(double *data, int size, const char* filename) return; } -void WritePointCloud(const char* data_path, - const char* color_filename, - const char* depth_filename, - unsigned int width, - unsigned int height, - const camera::PinholeCameraIntrinsic& intrinsic) -{ - geometry::Image img_color, img_depth; - - io::ReadImage(color_filename, img_color); - io::ReadImage(depth_filename, img_depth); - - utility::LogInfo("depth filename : {}", depth_filename); - utility::LogInfo("Reading RGBD image : "); - utility::LogInfo(" Color : {:d} x {:d} x {:d} ({:d} bits per channel)", - img_color.width_, img_color.height_, img_color.num_of_channels_, - img_color.bytes_per_channel_ * 8); - utility::LogInfo(" Depth : {:d} x {:d} x {:d} ({:d} bits per channel)", - img_depth.width_, img_depth.height_, img_depth.num_of_channels_, - img_depth.bytes_per_channel_ * 8); - double depth_scale = 1000.0, depth_trunc = 200.0; - bool convert_rgb_to_intensity = false; - std::shared_ptr rgbd_image = - geometry::RGBDImage::CreateFromColorAndDepth( - img_color, img_depth, depth_scale, depth_trunc, - convert_rgb_to_intensity); - - auto pcd = geometry::PointCloud::CreateFromRGBDImage(*rgbd_image, intrinsic); - std::string dataPath = std::string(data_path); - const std::string filename_ply(std::string(data_path) + std::string("/") + - dataPath.substr(dataPath.find_last_of("/\\") + 1) + - std::string(".ply")); - - utility::LogInfo("pcd counts : {}", pcd->points_.size()); -#ifndef BUILD_VISUALIZER - if (io::WritePointCloud(filename_ply, *pcd)) { - utility::LogInfo("Successfully wrote {}", filename_ply); - } else { - utility::LogError("Failed to write {}", filename_ply); - } -#else - unsigned int i, x, y, idx; - mv_source_h source = NULL; - int err = MEDIA_VISION_ERROR_NONE; - - cv::Mat rgb; - float *depth = NULL; - - Eigen::Vector3d min_xyz = pcd->GetMinBound(); - Eigen::Vector3d max_xyz = pcd->GetMaxBound(); - - int depth_width = (int)((max_xyz[0] - min_xyz[0]) * 100.0) + 1; - int depth_height = (int)((max_xyz[1] - min_xyz[1]) * 100.0) + 1; - - utility::LogInfo("min {}, max {}", min_xyz, max_xyz); - utility::LogInfo("depth W:H {}:{}", depth_width, depth_height); - - err = mv_create_source(&source); - if (MEDIA_VISION_ERROR_NONE != err) { - LOGE("Errors were occurred during creating the source!!! code : %i", err); - goto out; - } - depth = (float *)malloc(depth_width * depth_height * 4); - - rgb = cv::imread(color_filename, cv::IMREAD_COLOR); - cv::resize(rgb, rgb, cv::Size(depth_width, depth_height)); - cv::flip(rgb, rgb, 0); - cv::cvtColor(rgb, rgb, cv::COLOR_BGR2RGBA); - err = mv_source_fill_by_buffer(source, - rgb.ptr(), - rgb.elemSize() * rgb.size().width - * rgb.size().height, - rgb.size().width, - rgb.size().height, - MEDIA_VISION_COLORSPACE_RGB888); - if (MEDIA_VISION_ERROR_NONE != err) { - LOGE("Errors were occurred during filling the source!!! code : %i", err); - goto out; - } - - for (i = 0; i < pcd->points_.size(); i++) { - x = (int)((pcd->points_.data()[i][0] - min_xyz[0]) * 100.0); - y = (int)((max_xyz[1] - pcd->points_.data()[i][1]) * 100.0); - idx = y * depth_width + x; - depth[idx] = max_xyz[2] - pcd->points_.data()[i][2]; - } - if (source != NULL && depth != NULL) - while(1) - mv_util_visualizer_3d(source, depth, 100, 25); - -out: - if (depth != NULL) { - delete [] depth; - depth = NULL; - } - - if (source != NULL) { - err = mv_destroy_source(source); - if (MEDIA_VISION_ERROR_NONE != err) { - LOGE("Errors were occurred during destroying the source!!! code : %i", err); - } - } -#endif -} - void _depth_middlebury_cb(mv_source_h source, unsigned short* depth, unsigned int width, @@ -329,28 +225,34 @@ void _depth_middlebury_cb(mv_source_h source, udata->minDisp, udata->maxDisp); } - std::string intrinsic_path; - intrinsic_path = udata->intrinsicName; - utility::LogInfo("Camera intrinsic path {}", intrinsic_path); - - camera::PinholeCameraIntrinsic intrinsic; - if (intrinsic_path.empty() || - !io::ReadIJsonConvertible(intrinsic_path, intrinsic)) { - utility::LogWarning( - "Failed to read intrinsic parameters for depth image."); - utility::LogWarning("Using default value for Primesense camera."); - intrinsic = camera::PinholeCameraIntrinsic( - camera::PinholeCameraIntrinsicParameters::PrimeSenseDefault); + udata->diffMs = stopWatch.elapsedTime(); +} + +void _pointcloud_middlebury_cb(mv_source_h source, + mv_3d_pointcloud_h pointcloud, + void *user_data) +{ + if (!user_data) { + printf("user_data is null. Skip..\n"); + return; } - utility::LogInfo("focal_x = {}", intrinsic.GetFocalLength().first); - utility::LogInfo("focal_y = {}", intrinsic.GetFocalLength().second); - utility::LogInfo("cx = {}", intrinsic.GetPrincipalPoint().first); - utility::LogInfo("cy = {}", intrinsic.GetPrincipalPoint().second); - WritePointCloud(udata->dataPath.c_str(), udata->rgbName.c_str(), - udata->datasetName.c_str(), width, height, intrinsic); + auto udata = static_cast(user_data); + std::string filename = udata->dataset + std::string(".pcd"); + mv_3d_pointcloud_write_file(udata->mv3d, pointcloud, MV_3D_POINTCLOUD_TYPE_PCD_BIN, (char*)filename.c_str()); - udata->diffMs = stopWatch.elapsedTime(); + mv_3d_pointcloud_plane_model_h model; + mv_3d_pointcloud_plane_inlier_h inlier; + + mv_3d_pointcloud_plane_model_create(&model); + mv_3d_pointcloud_plane_inlier_create(&inlier); + mv_3d_pointcloud_segment_plane(udata->mv3d, pointcloud, &model, &inlier); + + filename = udata->dataset + std::string("-plane.pcd"); + mv_3d_pointcloud_plane_write_file(udata->mv3d, model, inlier, pointcloud, MV_3D_POINTCLOUD_TYPE_PCD_BIN, (char*)filename.c_str()); + + mv_3d_pointcloud_plane_model_destroy(model); + mv_3d_pointcloud_plane_inlier_destroy(inlier); } int perform_middlebury_test() @@ -405,6 +307,10 @@ int perform_middlebury_test() char leftFilename[MAX_STRING_LENGTH]; char rightFilename[MAX_STRING_LENGTH]; char stereoConfigFileName[MAX_STRING_LENGTH]; + double samplingRatio = 0.01; + int outlierRemovalPoints = 3; + double outlierRemovalRadius = 0.5; + snprintf(dataPath, MAX_STRING_LENGTH, "%s/%s", path_to_dataset, dataset[data]); snprintf(calibFilename, MAX_STRING_LENGTH, "%s/calib.txt", dataPath); snprintf(stereoConfigFileName, MAX_STRING_LENGTH, "%s/calibOcv.xml", dataPath); @@ -481,9 +387,37 @@ int perform_middlebury_test() goto _err; } + err = mv_engine_config_set_double_attribute(engine_config, + MV_3D_POINTCLOUD_SAMPLING_RATIO, samplingRatio); + if (err != MEDIA_VISION_ERROR_NONE) { + printf("Failed to set stereo config file path\n"); + goto _err; + } + + err = mv_engine_config_set_int_attribute(engine_config, + MV_3D_POINTCLOUD_OUTLIER_REMOVAL_POINTS, outlierRemovalPoints); + if (err != MEDIA_VISION_ERROR_NONE) { + printf("Failed to set stereo config file path\n"); + goto _err; + } + + err = mv_engine_config_set_double_attribute(engine_config, + MV_3D_POINTCLOUD_OUTLIER_REMOVAL_RADIUS, outlierRemovalRadius); + if (err != MEDIA_VISION_ERROR_NONE) { + printf("Failed to set stereo config file path\n"); + goto _err; + } + + err = mv_engine_config_set_string_attribute(engine_config, + MV_3D_POINTCLOUD_OUTPUT_FILE_PATH, MEDIA_FILE_PATH); + if (err != MEDIA_VISION_ERROR_NONE) { + printf("Failed to set stereo config file path\n"); + goto _err; + } + // left_source, right_source - left_frame = cv::imread(leftFilename, cv::IMREAD_GRAYSCALE); - right_frame = cv::imread(rightFilename, cv::IMREAD_GRAYSCALE); + left_frame = cv::imread(leftFilename, cv::IMREAD_COLOR); + right_frame = cv::imread(rightFilename, cv::IMREAD_COLOR); err = mv_source_fill_by_buffer(left_source, left_frame.ptr(), @@ -491,7 +425,7 @@ int perform_middlebury_test() * left_frame.size().height, left_frame.size().width, left_frame.size().height, - MEDIA_VISION_COLORSPACE_Y800); + MEDIA_VISION_COLORSPACE_RGB888); if (err != MEDIA_VISION_ERROR_NONE) { printf("Failed to fill left_source\n"); goto _err; @@ -503,7 +437,7 @@ int perform_middlebury_test() * right_frame.size().height, right_frame.size().width, right_frame.size().height, - MEDIA_VISION_COLORSPACE_Y800); + MEDIA_VISION_COLORSPACE_RGB888); if (err != MEDIA_VISION_ERROR_NONE) { printf("Failed to fill right_source\n"); goto _err; @@ -524,6 +458,7 @@ int perform_middlebury_test() // get depth appdata dump { + mv3d_handle, std::chrono::milliseconds::zero(), std::string(dataPath), std::string(dataPath) + std::string("/intrinsics.json"), @@ -534,10 +469,17 @@ int perform_middlebury_test() sel_fmt }; dump.datasetName += sel_fmt == FMT_PFM ? std::string(".pfm") : std::string(".png"); + dump.dataset = std::string(dataset[data]); err = mv_3d_set_depth_cb(mv3d_handle, _depth_middlebury_cb, static_cast(&dump)); if (err != MEDIA_VISION_ERROR_NONE) { - printf("Failed to set callback to depth handle\n"); + printf("Failed to set depth callback to handle\n"); + goto _err; + } + + err = mv_3d_set_pointcloud_cb(mv3d_handle, _pointcloud_middlebury_cb, static_cast(&dump)); + if (err != MEDIA_VISION_ERROR_NONE) { + printf("Failed to set pointcloud callback to handle\n"); goto _err; } @@ -585,8 +527,6 @@ int main() int err = MEDIA_VISION_ERROR_NONE; const char* names[] = {"Middlebury - TrainingQ(Imperf)"}; - utility::SetVerbosityLevel(utility::VerbosityLevel::Debug); - int sel_opt = show_menu_linear("Select Action", names, ARRAY_SIZE(names)); if (sel_opt <= 0 || sel_opt > ARRAY_SIZE(names)) { printf("Invalid option"); diff --git a/test/testsuites/mv3d/depthstream_test_suite.cpp b/test/testsuites/mv3d/depthstream_test_suite.cpp index c2441bc2..4051ef5d 100644 --- a/test/testsuites/mv3d/depthstream_test_suite.cpp +++ b/test/testsuites/mv3d/depthstream_test_suite.cpp @@ -370,10 +370,10 @@ static void stereo_handoff(GstElement *object, GstBuffer *buffer, GstPad *pad, gst_buffer_unmap(buffer, &map); if (isAsync) { - ret = mv_3d_run_async(mv3d_handle, mv_source, NULL, NULL); + ret = mv_3d_run_async(mv_source, NULL, NULL, mv3d_handle); __wait(); } else { - ret = mv_3d_run(mv3d_handle, mv_source, NULL, NULL); + ret = mv_3d_run(mv_source, NULL, NULL, mv3d_handle); } if (ret != MEDIA_VISION_ERROR_NONE) { -- cgit v1.2.3