Implemented maks based point cloud segmentation.

CMakeLists.txt

@@ -7,6 +7,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/ai/impl/fast_sam.cpp
         src/ai/impl/zmq_client.cpp
         src/image_process/impl/image_process_ui.cpp
+        src/image_process/impl/process_funcs.cpp
         src/image_process/impl/versatile_convertor.cpp
         src/impl/main_impl.cpp
         src/impl/apps/app_selector/app_selector.cpp

src/ai/impl/zmq_client.cpp

@@ -142,6 +142,7 @@ zmq_client::impl::impl(const create_config &conf) {
             conf.python_interpreter, conf.server_script_path,
             aux_env, bp::start_dir(conf.server_working_dir));
 
+    // create auxiliary thread
     aux_ctx = std::make_unique<io_context>();
     aux_thread = std::make_unique<std::thread>([=, this]() {
         aux_thread_work(conf);

src/core/impl/image_utility_v2.cpp

@@ -13,8 +13,11 @@ namespace image_utility_impl {
             case CV_8UC4: { return typeid(uchar4); }
             case CV_16UC1: { return typeid(ushort1); }
             case CV_32FC1: { return typeid(float1); }
-            default: { RET_ERROR; }
-                // @formatter:on
+            case CV_32FC2: { return typeid(float2); }
+            // @formatter:on
+            default: {
+                RET_ERROR;
+            }
         }
     }
 
@@ -372,6 +375,7 @@ template image_type_v2<uchar3> generic_image::cuda(smart_cuda_stream *stream);
 template image_type_v2<uchar4> generic_image::cuda(smart_cuda_stream *stream);
 template image_type_v2<ushort1> generic_image::cuda(smart_cuda_stream *stream);
 template image_type_v2<float1> generic_image::cuda(smart_cuda_stream *stream);
+template image_type_v2<float2> generic_image::cuda(smart_cuda_stream *stream);
 // @formatter:on
 
 template<typename T>

src/core/impl/pc_utility.cpp

@@ -1,5 +1,21 @@
 #include "pc_utility_impl.h"
 
+namespace pc_utility_impl {
+
+    std::type_index fmt_type_id(pc_format_enum fmt) {
+        switch (fmt) {
+            // @formatter:off
+            case PC_XYZ: { return typeid(pc_xyz_type); }
+            case PC_XYZ_RGB: { return typeid(pc_xyz_rgb_type); }
+            // @formatter:on
+            default: {
+                RET_ERROR;
+            }
+        }
+    }
+
+}
+
 void *pc_memory::at(size_t idx) {
     return (uint8_t *) start_ptr() + pc->elem_size() * idx;
 }
@@ -90,6 +106,14 @@ pc_memory generic_pc::impl::get_memory(memory_location loc,
     return ret;
 }
 
+template<typename PointT>
+pc_type_v1<PointT> generic_pc::impl::get_pc_type_v1(smart_cuda_stream *stream) {
+    create_cuda(stream);
+    assert(fmt_type_id(fmt) == typeid(PointT));
+    assert(size <= std::numeric_limits<uint>::max());
+    return pc_type_v1<PointT>((PointT *) store_cuda.ptr.get(), size);
+}
+
 void generic_pc::impl::host_modified(smart_cuda_stream *stream) {
     assert(store_host.ptr != nullptr);
     store_cuda.reset();
@@ -139,6 +163,16 @@ pc_memory generic_pc::memory(memory_location loc, smart_cuda_stream *stream) {
     return pimpl->get_memory(loc, stream);
 }
 
+template<typename PointT>
+pc_type_v1<PointT> generic_pc::cuda(smart_cuda_stream *stream) {
+    return pimpl->get_pc_type_v1<PointT>(stream);
+}
+
+// @formatter:off
+template pc_type_v1<pc_xyz_type> generic_pc::cuda(smart_cuda_stream *stream);
+template pc_type_v1<pc_xyz_rgb_type> generic_pc::cuda(smart_cuda_stream *stream);
+// @formatter:on
+
 void generic_pc::host_modified(smart_cuda_stream *stream) {
     pimpl->host_modified(stream);
 }

src/core/impl/pc_utility_impl.h

@@ -3,6 +3,16 @@
 
 #include "core/pc_utility.h"
 
+#include <typeindex>
+
+namespace pc_utility_impl {
+
+    std::type_index fmt_type_id(pc_format_enum fmt);
+
+}
+
+using namespace pc_utility_impl;
+
 struct generic_pc::impl : public meta_base::impl {
 
     generic_pc *q_this = nullptr;
@@ -34,6 +44,9 @@ struct generic_pc::impl : public meta_base::impl {
     pc_memory get_memory(memory_location loc,
                          smart_cuda_stream *stream);
 
+    template<typename PointT>
+    pc_type_v1<PointT> get_pc_type_v1(smart_cuda_stream *stream);
+
     void host_modified(smart_cuda_stream *stream);
 
     void cuda_modified(smart_cuda_stream *stream);

src/core/pc_utility.h

@@ -4,6 +4,7 @@
 #include "core/cuda_helper.hpp"
 #include "core/memory_pool.h"
 #include "core/meta_helper.hpp"
+#include "image_process/cuda_impl/pc_utility.cuh"
 
 #include <memory>
 
@@ -12,15 +13,6 @@ enum pc_format_enum {
     PC_XYZ_RGB
 };
 
-struct pc_xyz_type {
-    float x, y, z;
-};
-
-struct pc_xyz_rgb_type {
-    float x, y, z;
-    uint8_t r, g, b;
-}; // TODO: try __attribute__((packed))
-
 class generic_pc;
 
 struct pc_memory {
@@ -68,6 +60,9 @@ public:
     pc_memory memory(memory_location loc,
                      smart_cuda_stream *stream = nullptr);
 
+    template<typename PointT>
+    pc_type_v1<PointT> cuda(smart_cuda_stream *stream = nullptr);
+
     void host_modified(smart_cuda_stream *stream = nullptr);
 
     void cuda_modified(smart_cuda_stream *stream = nullptr);

src/device/impl/orb_camera.cpp

@@ -1,13 +1,18 @@
 #include "orb_camera_impl.h"
 #include "core/image_utility.hpp"
+#include "core/image_utility_v2.h"
 #include "core/object_manager.h"
 #include "core/utility.hpp"
 
 #include <boost/asio/post.hpp>
 
+#include <opencv2/calib3d.hpp>
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/imgproc.hpp>
 
+#include <glm/glm.hpp>
+#include <glm/gtc/type_ptr.hpp>
+
 using boost::asio::post;
 
 namespace orb_camera_impl {
@@ -102,6 +107,8 @@ orb_camera::impl *orb_camera::impl::create(orb_camera::create_config conf) {
     ret->c_pf_list = ret->pipe->getStreamProfileList(OB_SENSOR_COLOR);
     ret->d_pf_list = ret->pipe->getStreamProfileList(OB_SENSOR_DEPTH);
 
+    ret->remap_name = conf.remap_name;
+
     return ret;
 }
 
@@ -109,6 +116,8 @@ orb_camera::impl::~impl() {
     if (is_capturing) {
         stop();
     }
+
+    aux_thread->join();
 }
 
 orb_camera::vi_list_ptr orb_camera::impl::create_video_info_list(const pf_list_type &pf_list) {
@@ -155,6 +164,44 @@ orb_camera::vi_list_ptr orb_camera::impl::query_d2c_info(uint32_t c_conf_index)
     return create_video_info_list(d_pf);
 }
 
+void orb_camera::impl::generate_undistort_map() { // TODO: accelerate with CUDA
+    if (remap_name == invalid_obj_name) return;
+
+    auto cam_params = pipe->getCameraParam();
+//    assert(cam_params.rgbIntrinsic == cam_params.depthIntrinsic);
+//    assert(cam_params.rgbDistortion == cam_params.depthDistortion);
+    auto cam_in = cam_params.rgbIntrinsic;
+    // @formatter:off
+    cv::Mat cam_mat_cv = (cv::Mat_<float>(3, 3) <<
+            cam_in.fx, 0.f, cam_in.cx,
+            0, cam_in.fy, cam_in.cy,
+            0, 0, 1);
+    // @formatter:on
+
+    auto cam_dist = cam_params.rgbDistortion;
+    // @formatter:off
+    cv::Mat dist_cv = (cv::Mat_<float>(8, 1) <<
+            cam_dist.k1, cam_dist.k2, cam_dist.p1, cam_dist.p2,
+            cam_dist.k3, cam_dist.k4, cam_dist.k5, cam_dist.k6);
+    // @formatter:on
+
+    auto img_size = cv::Size(cam_in.width, cam_in.height);
+    auto points_size = cv::Size(img_size.area(), 1);
+    auto points_mat = cv::Mat(points_size, CV_32FC2);
+    for (auto iy = 0; iy < cam_in.height; ++iy)
+        for (auto ix = 0; ix < cam_in.width; ++ix) {
+            auto id = iy * cam_in.width + ix;
+            points_mat.at<cv::Vec2f>(id) = cv::Vec2f(ix, iy);
+        }
+
+    auto undistort_img = create_image(img_size, CV_32FC2);
+    auto data = undistort_img->memory(MEM_HOST).start_ptr();
+    auto undistort_mat = cv::Mat(points_size, CV_32FC2, data);
+    cv::undistortPoints(points_mat, undistort_mat, cam_mat_cv, dist_cv);
+    undistort_img->host_modified();
+    OBJ_SAVE(remap_name, undistort_img);
+}
+
 bool orb_camera::impl::start(start_config conf) {
     assert(!is_capturing);
     auto ob_conf = std::make_shared<ob::Config>();
@@ -190,12 +237,17 @@ bool orb_camera::impl::start(start_config conf) {
     // Example says that the filter depends on the started pipeline.
     if (conf.pc.enable) {
         pc_filter = std::make_shared<ob::PointCloudFilter>();
+        auto params = pipe->getCameraParam();
         pc_filter->setCameraParam(pipe->getCameraParam());
         pc_filter->setCreatePointFormat(OB_FORMAT_RGB_POINT);
         pc_filter->setCallBack([this](auto frame) { pc_callback(frame); });
         pc_name = conf.pc.name;
     }
 
+    // open a new thread to do the time-consuming calculation work
+    aux_thread = std::make_unique<std::thread>(
+            [this] { generate_undistort_map(); });
+
     return true;
 }
 

src/device/impl/orb_camera_impl.h

@@ -48,6 +48,9 @@ struct orb_camera::impl {
     obj_name_type d_name = invalid_obj_name;
     obj_name_type pc_name = invalid_obj_name;
 
+    obj_name_type remap_name = invalid_obj_name;
+    std::unique_ptr<std::thread> aux_thread;
+
     static constexpr auto align_mode = ALIGN_D2C_HW_MODE;
 
     bool is_capturing = false;
@@ -64,6 +67,8 @@ struct orb_camera::impl {
 
     vi_list_ptr query_d2c_info(uint32_t c_conf_index);
 
+    void generate_undistort_map();
+
     bool start(start_config conf);
 
     using frames_type = std::shared_ptr<ob::FrameSet>;

src/device/impl/orb_camera_ui.cpp

@@ -11,6 +11,7 @@ using boost::asio::post;
 
 orb_camera_ui::impl::impl(create_config conf) {
     ctx = conf.ctx;
+    cam_c_conf.remap_name = conf.remap_name;
     cam_c_conf.stream = conf.stream;
     cam_c_conf.ctx = ctx;
 

src/device/orb_camera.h

@@ -25,6 +25,13 @@ public:
 
     struct create_config {
         const char *sn_str = nullptr; // serial number
+
+        // image_ptr, CV_32FC2
+        // an image that maps each pixel to its undistorted position in the normalized plane
+        obj_name_type remap_name = invalid_obj_name;
+
+//        obj_name_type camera_intrinsic_name = invalid_obj_name; // camera_intrinsic_v2
+
         smart_cuda_stream *stream = nullptr;
         boost::asio::io_context *ctx = nullptr;
     };

src/device/orb_camera_ui.h

@@ -13,6 +13,7 @@ public:
         obj_name_type cf_name = invalid_obj_name; // color frame name
         obj_name_type df_name = invalid_obj_name; // depth frame name
         obj_name_type pc_name = invalid_obj_name; // point cloud name
+        obj_name_type remap_name = invalid_obj_name;
         boost::asio::io_context *ctx = nullptr;
         smart_cuda_stream *stream = default_cuda_stream;
     };

src/image_process/camera_utility.hpp

@@ -28,6 +28,13 @@ struct camera_intrinsic {
 
 };
 
+struct camera_intrinsic_v2 { // for Orbbec Camera
+    float fx, fy; // focus length in pixel
+    float cx, cy; // optical center in pixel
+    float k[6], p[2]; // distort coefficients
+    uint32_t width, height;
+};
+
 inline Eigen::Isometry3f transform_from_yaml_f(const YAML::Node &conf) {
     return Eigen::Translation3f(
             LOAD_NUMBER(float, "tx"),

src/image_process/cuda_impl/CMakeLists.txt

@@ -6,6 +6,8 @@ set(CMAKE_CXX_STANDARD 20)
 add_library(${PROJECT_NAME}
         fake_color.cu
         image_merge.cu
+        pc_filter.cu
+        pc_generate.cu
         pixel_convert.cu)
 
 # CUDA config

src/image_process/cuda_impl/kernel_utility.cuh

@@ -43,13 +43,14 @@ __global__ void image_elementwise_binary_in(image_type_v2<PixIn1> in1, image_typ
     }
 }
 
+// https://github.com/Oneflow-Inc/oneflow/blob/master/oneflow/core/cuda/elementwise.cuh
+static constexpr auto max_cuda_grids = 4352; // TODO: calculate by hardware at runtime
+
 inline std::tuple<dim3, dim3> get_kernel_size(ushort width, ushort height) {
     static constexpr auto block_x = 32;
     static constexpr auto block_y = 8;
-    // https://github.com/Oneflow-Inc/oneflow/blob/master/oneflow/core/cuda/elementwise.cuh
-    static constexpr auto max_grids = 4352; // TODO: calculate by hardware at runtime
-    auto grid_y = std::max<uint>(1, std::min<uint>(height / block_y, max_grids));
-    auto grid_x = std::max<uint>(1, std::min<uint>(width / block_x, max_grids / grid_y));
+    auto grid_y = std::max<uint>(1, std::min<uint>(height / block_y, max_cuda_grids));
+    auto grid_x = std::max<uint>(1, std::min<uint>(width / block_x, max_cuda_grids / grid_y));
     auto block_dim = dim3(block_x, block_y, 1);
     auto grid_dim = dim3(grid_x, grid_y, 1);
     return std::make_tuple(grid_dim, block_dim);

src/image_process/cuda_impl/pc_filter.cu

@@ -0,0 +1,34 @@
+#include "pc_filter.cuh"
+
+#include <thrust/copy.h>
+
+namespace pc_filter {
+
+    template<typename PointT>
+    struct non_zero_depth_op {
+        __host__ __device__
+        constexpr bool operator()(const PointT &p) {
+            return p.z != 0;
+        }
+    };
+
+    template<typename PointT>
+    void shrink_zero_depth(pc_type_v1<PointT> in, pc_type_v1<PointT> out,
+                           size_t *out_num, cudaStream_t stream) {
+        assert(out.num >= in.num);
+        auto end_iter = thrust::copy_if(thrust::cuda::par.on(stream),
+                                        in.ptr, in.ptr + in.num, out.ptr,
+                                        non_zero_depth_op<PointT>());
+        if (out_num != nullptr) {
+            *out_num = end_iter - out.ptr;
+        } else {
+            assert(false);
+        }
+    }
+
+    // @formatter:off
+    template void shrink_zero_depth<pc_xyz_type>(pc_type_xyz, pc_type_xyz, size_t *, cudaStream_t);
+    template void shrink_zero_depth<pc_xyz_rgb_type>(pc_type_rgb , pc_type_rgb, size_t *, cudaStream_t);
+    // @formatter:on
+
+}

src/image_process/cuda_impl/pc_filter.cuh

@@ -0,0 +1,14 @@
+#ifndef DEPTHGUIDE_PC_FILTER_CUH
+#define DEPTHGUIDE_PC_FILTER_CUH
+
+#include "pc_utility.cuh"
+
+namespace pc_filter {
+
+    template<typename PointT>
+    void shrink_zero_depth(pc_type_v1<PointT> in, pc_type_v1<PointT> out,
+                           size_t *out_num, cudaStream_t stream);
+
+}
+
+#endif //DEPTHGUIDE_PC_FILTER_CUH

src/image_process/cuda_impl/pc_generate.cu

@@ -0,0 +1,67 @@
+#include "pc_generate.cuh"
+#include "kernel_utility.cuh"
+
+__global__ void gen_rgb_with_mask(image_type_v2<uchar3> color,
+                                  image_type_v2<float1> depth,
+                                  image_type_v2<uchar1> mask,
+                                  image_type_v2<float2> remap,
+                                  pc_type_rgb out) {
+
+    auto width = color.width, height = color.height;
+    for (auto idy = blockDim.y * blockIdx.y + threadIdx.y;
+         idy < height;
+         idy += gridDim.y * blockDim.y) {
+
+        for (auto idx = blockDim.x * blockIdx.x + threadIdx.x;
+             idx < width;
+             idx += gridDim.x * blockDim.x) {
+
+            auto id = idy * width + idx;
+
+#define IGNORE_PIX \
+            out.at(id)->z = 0; \
+            continue
+
+            if (mask.at(idy, idx)->x == 0) { IGNORE_PIX; }
+
+            auto pix_depth = depth.at(idy, idx)->x;
+            if (pix_depth == 0) { IGNORE_PIX; }
+
+#undef IGNORE_PIX
+
+            auto pix_xy = *remap.at(idy, idx);
+            auto pix_out = pc_xyz_rgb_type();
+            pix_out.x = pix_xy.x * pix_depth;
+            pix_out.y = pix_xy.y * pix_depth;
+            pix_out.z = pix_depth;
+
+            auto pix_rgb = *color.at(idy, idx);
+            pix_out.r = pix_rgb.x;
+            pix_out.g = pix_rgb.y;
+            pix_out.b = pix_rgb.z;
+
+            *out.at(id) = pix_out;
+        }
+    }
+}
+
+namespace gen_pc {
+
+    void call_rgb_with_mask(image_type_v2<uchar3> color, image_type_v2<float1> depth,
+                            image_type_v2<uchar1> mask, image_type_v2<float2> remap,
+                            pc_type_rgb out, cudaStream_t stream) {
+        assert(depth.width == color.width);
+        assert(mask.width == color.width);
+        assert(remap.width == color.width);
+        assert(depth.height == color.height);
+        assert(mask.height == color.height);
+        assert(remap.height == color.height);
+        assert(out.num == color.width * color.height);
+
+        auto [grid_dim, block_dim] = get_kernel_size(color.width, color.height);
+        gen_rgb_with_mask<<<grid_dim, block_dim, 0, stream>>>(
+                color, depth, mask, remap, out);
+    }
+
+}
+

src/image_process/cuda_impl/pc_generate.cuh

@@ -0,0 +1,15 @@
+#ifndef DEPTHGUIDE_PC_GENERATE_CUH
+#define DEPTHGUIDE_PC_GENERATE_CUH
+
+#include "pc_utility.cuh"
+#include "image_utility.cuh"
+
+namespace gen_pc {
+
+    void call_rgb_with_mask(image_type_v2<uchar3> color, image_type_v2<float1> depth,
+                            image_type_v2<uchar1> mask, image_type_v2<float2> remap,
+                            pc_type_rgb out, cudaStream_t stream);
+
+}
+
+#endif //DEPTHGUIDE_PC_GENERATE_CUH

src/image_process/cuda_impl/pc_utility.cuh

@@ -0,0 +1,29 @@
+#ifndef DEPTHGUIDE_PC_UTILITY_CUH
+#define DEPTHGUIDE_PC_UTILITY_CUH
+
+#include <cassert>
+#include <cstdint>
+
+template<typename PointT>
+struct pc_type_v1 {
+    PointT *ptr = nullptr;
+    uint num = 0; // number of points
+
+    __device__ __host__ auto at(uint x) const {
+        return (PointT *) ptr + x;
+    }
+};
+
+struct pc_xyz_type {
+    float x, y, z;
+};
+
+struct pc_xyz_rgb_type {
+    float x, y, z;
+    uint8_t r, g, b;
+}; // TODO: try __attribute__((packed))
+
+using pc_type_xyz = pc_type_v1<pc_xyz_type>;
+using pc_type_rgb = pc_type_v1<pc_xyz_rgb_type>;
+
+#endif //DEPTHGUIDE_PC_UTILITY_CUH

src/image_process/impl/process_funcs.cpp

@@ -0,0 +1,64 @@
+#include "image_process/process_funcs.h"
+#include "core/image_utility_v2.h"
+#include "core/pc_utility.h"
+#include "image_process/cuda_impl/pc_generate.cuh"
+#include "image_process/cuda_impl/pc_filter.cuh"
+
+namespace gen_pc_rgb_with_mask {
+
+    void call(config conf) {
+        auto color_img = OBJ_QUERY(image_u8c3, conf.color_name);
+        auto depth_img = OBJ_QUERY(image_f32c1, conf.depth_name);
+        auto mask_img = OBJ_QUERY(image_ptr, conf.mask_name);
+        auto remap_img = OBJ_QUERY(image_ptr, conf.remap_name);
+
+        if (remap_img == nullptr || mask_img == nullptr) return;
+        assert(color_img != nullptr && depth_img != nullptr);
+
+        auto stream = conf.stream;
+        auto color = color_img->as_cuda(stream);
+        auto depth = depth_img->as_cuda(stream);
+        auto mask = mask_img->cuda<uchar1>(stream);
+        auto remap = remap_img->cuda<float2>(stream);
+        auto out_pc = create_pc(color_img->size().area(), PC_XYZ_RGB);
+        auto pc_info = out_pc->cuda<pc_xyz_rgb_type>(stream);
+
+        gen_pc::call_rgb_with_mask(
+                color, depth, mask, remap, pc_info, stream->cuda);
+        out_pc->cuda_modified(stream);
+        OBJ_SAVE(conf.pc_out_name, out_pc);
+    }
+
+}
+
+namespace shrink_pc_zero_depth {
+
+    template<typename PointT>
+    void call_impl(config conf, const pc_ptr &pc_in) {
+        auto stream = conf.stream;
+        auto in_info = pc_in->cuda<PointT>(stream);
+        auto pc_out = create_pc(pc_in->size(), pc_in->format());
+        size_t out_num = 0;
+        pc_filter::shrink_zero_depth(in_info, pc_out->cuda<PointT>(stream),
+                                     &out_num, conf.stream->cuda);
+        pc_out->shrink(out_num);
+        pc_out->cuda_modified(stream);
+        OBJ_SAVE(conf.out_name, pc_out);
+    }
+
+    void call(config conf) {
+        auto pc_in = OBJ_QUERY(pc_ptr, conf.in_name);
+        if (pc_in == nullptr) return;
+
+        switch (pc_in->format()) {
+            // @formatter:off
+            case PC_XYZ: { call_impl<pc_xyz_type>(conf, pc_in); break; }
+            case PC_XYZ_RGB: { call_impl<pc_xyz_rgb_type>(conf, pc_in); break; }
+            // @formatter:off
+            default: {
+                RET_ERROR;
+            }
+        }
+    }
+
+}

src/image_process/process_funcs.h

@@ -0,0 +1,35 @@
+#ifndef DEPTHGUIDE_PROCESS_FUNCS_H
+#define DEPTHGUIDE_PROCESS_FUNCS_H
+
+#include "core/cuda_helper.hpp"
+#include "core/object_manager.h"
+
+namespace gen_pc_rgb_with_mask {
+
+    struct config {
+        obj_name_type color_name;
+        obj_name_type depth_name;
+        obj_name_type mask_name;
+        obj_name_type remap_name;
+        obj_name_type pc_out_name;
+        smart_cuda_stream *stream;
+    };
+
+    void call(config conf);
+
+}
+
+namespace shrink_pc_zero_depth {
+
+    struct config {
+        obj_name_type in_name;
+        obj_name_type out_name;
+        smart_cuda_stream *stream;
+    };
+
+    void call(config conf);
+
+}
+
+
+#endif //DEPTHGUIDE_PROCESS_FUNCS_H

src/image_process/versatile_convertor.h

@@ -17,7 +17,9 @@ enum convert_options {
     CVT_FAKE_DECODE_555P,
     CVT_FAKE_DECODE_800P,
 
-    CVT_HALF_SPLIT
+    CVT_HALF_SPLIT,
+
+    CVT_PC_FILTER_MASK,
 };
 
 class versatile_convertor {

src/image_process_v3/image_process.h

@@ -8,6 +8,7 @@
 
 // return 1/4 part of valid ranges
 // angle: view angle
+// TODO: replace with cv::getOptimalNewCameraMatrix
 cv::Size2f calc_valid_range(const camera_intrinsic &left,
                             const camera_intrinsic &right,
                             float *angle = nullptr);

src/impl/apps/depth_guide/depth_guide.cpp

@@ -3,6 +3,7 @@
 #include "core/imgui_utility.hpp"
 #include "core/pc_utility.h"
 #include "image_process/impl/versatile_convertor_impl.h"
+#include "image_process/process_funcs.h"
 
 app_depth_guide::app_depth_guide(const create_config &_conf) {
     conf = _conf;
@@ -10,16 +11,30 @@ app_depth_guide::app_depth_guide(const create_config &_conf) {
     // initialize object manager
     OBJ_SAVE(img_color, image_u8c3());
     OBJ_SAVE(img_depth, image_f32c1());
+    OBJ_SAVE(img_remap, image_ptr());
     OBJ_SAVE(img_depth_fake, image_u8c3());
     auto fake_info = fake_color_config{.mode = FAKE_800P, .lower = 200, .upper = 1000};
     OBJ_SAVE(img_depth_fake_info, versatile_convertor_impl::encode_config(fake_info));
     OBJ_SAVE(img_out, image_u8c4());
     OBJ_SAVE(img_obj_mask, image_ptr());
     OBJ_SAVE(pc_raw, pc_ptr());
+    OBJ_SAVE(pc_obj_raw, pc_ptr());
+    OBJ_SAVE(pc_obj, pc_ptr());
+
+    OBJ_SIG(img_obj_mask)->connect([](auto _) {
+        gen_pc_rgb_with_mask::call(
+                {img_color, img_depth, img_obj_mask,
+                 img_remap, pc_obj_raw, default_cuda_stream});
+    });
+    OBJ_SIG(pc_obj_raw)->connect([](auto _) {
+        shrink_pc_zero_depth::call(
+                {pc_obj_raw, pc_obj, default_cuda_stream});
+    });
 
     // initialize modules
     auto orb_cam_conf = orb_camera_ui::create_config{
-            .cf_name = img_color, .df_name = img_depth, .pc_name = pc_raw,
+            .cf_name = img_color, .df_name = img_depth,
+            .pc_name = pc_raw, .remap_name = img_remap,
             .ctx = conf.asio_ctx, .stream = default_cuda_stream,
     };
     orb_cam = std::make_unique<orb_camera_ui>(orb_cam_conf);
@@ -65,10 +80,11 @@ app_depth_guide::app_depth_guide(const create_config &_conf) {
     auto saver_conf = image_saver::create_config{.ctx = conf.asio_ctx};
     saver_conf.img_list.emplace_back("Image", img_color);
     saver_conf.img_list.emplace_back("Image Out", img_out);
+    saver_conf.img_list.emplace_back("PC Raw", pc_raw);
     out_saver = std::make_unique<image_saver>(saver_conf);
 
     auto pc_conf = pc_viewer::create_config{
-            .name = pc_raw, .stream = default_cuda_stream,
+            .name = pc_obj, .stream = default_cuda_stream,
     };
     scene_viewer = std::make_unique<pc_viewer>(pc_conf);
 }

src/impl/apps/depth_guide/depth_guide.h

@@ -33,7 +33,7 @@ private:
     enum obj_names : object_manager::name_type {
 
         // images from device
-        img_color, img_depth,
+        img_color, img_depth, img_remap,
 
         // depth with fake color
         img_depth_fake, img_depth_fake_info,
@@ -45,7 +45,10 @@ private:
         pc_raw,
 
         // ai segment result
-        img_obj_mask
+        img_obj_mask,
+
+        // object point cloud
+        pc_obj_raw, pc_obj,
     };
 
     create_config conf;

src/module/impl/image_saver.cpp

@@ -1,6 +1,7 @@
 #include "image_saver_impl.h"
 #include "core/image_utility.hpp"
 #include "core/imgui_utility.hpp"
+#include "core/pc_utility.h"
 
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/imgproc.hpp>
@@ -13,6 +14,36 @@
 
 using boost::asio::post;
 
+void image_saver::impl::item_store_type::save_ply() {
+    auto file_name = fmt::format("{}_{}.ply", ui_name, par->save_cnt);
+    auto pc = OBJ_QUERY(pc_ptr, img_name);
+    assert(pc->format() == PC_XYZ_RGB);
+
+    auto size = pc->size();
+    FILE *fp = fopen(file_name.c_str(), "wb+"); // TODO: move save function to pc_utility
+    fprintf(fp, "ply\n");
+    fprintf(fp, "format ascii 1.0\n");
+    fprintf(fp, "element vertex %ld\n", size);
+    fprintf(fp, "property float x\n");
+    fprintf(fp, "property float y\n");
+    fprintf(fp, "property float z\n");
+    fprintf(fp, "property uchar red\n");
+    fprintf(fp, "property uchar green\n");
+    fprintf(fp, "property uchar blue\n");
+    fprintf(fp, "end_header\n");
+
+    auto pc_mem = pc->memory(MEM_HOST, &par->stream);
+    auto ptr = (pc_xyz_rgb_type *) pc_mem.start_ptr();
+    for (int i = 0; i < size; i++) {
+        fprintf(fp, "%.3f %.3f %.3f %d %d %d\n",
+                ptr->x, ptr->y, ptr->z, ptr->r, ptr->g, ptr->b);
+        ptr++;
+    }
+
+    fflush(fp);
+    fclose(fp);
+}
+
 void image_saver::impl::item_store_type::save_png() {
     auto file_name = fmt::format("{}_{}.png", ui_name, par->save_cnt);
     auto img_mat = image_as_mat(img_name, &par->stream);
@@ -33,6 +64,7 @@ void image_saver::impl::item_store_type::process() { // TODO: move save work to
         case SAVE_PNG: { save_png(); break; }
 //        case SAVE_JPG: { save_jpg(); break; }
 //        case SAVE_RAW: { save_raw(); break; }
+        case SAVE_PLY: { save_ply(); break; }
         // @formatter:on
     }
 }
@@ -42,9 +74,13 @@ image_saver::impl::impl(const create_config &conf) {
     std::ranges::transform(
             conf.img_list, std::back_inserter(item_list),
             [par = this](auto item) {
+                auto is_point_cloud =
+                        OBJ_TYPE(item.img_name) == typeid(pc_ptr);
                 return item_store_type{
                         .ui_name = item.name,
                         .img_name = item.img_name,
+                        .data_type = is_point_cloud ? DATA_PC : DATA_IMG,
+                        .save_type = is_point_cloud ? SAVE_PLY : SAVE_PNG,
                         .par = par};
             });
 }
@@ -77,13 +113,18 @@ void image_saver::impl::show() {
 
             // save type
             ImGui::SameLine();
-            ImGui::RadioButton("PNG", &item.save_type, SAVE_PNG);
 
-            auto guard = imgui_disable_guard(); // not implemented
-            ImGui::SameLine();
-            ImGui::RadioButton("JPG", &item.save_type, SAVE_JPG);
-            ImGui::SameLine();
-            ImGui::RadioButton("RAW", &item.save_type, SAVE_RAW);
+            if (item.data_type == DATA_IMG) {
+                ImGui::RadioButton("PNG", &item.save_type, SAVE_PNG);
+                auto guard = imgui_disable_guard(); // not implemented
+                ImGui::SameLine();
+                ImGui::RadioButton("JPG", &item.save_type, SAVE_JPG);
+                ImGui::SameLine();
+                ImGui::RadioButton("RAW", &item.save_type, SAVE_RAW);
+            } else {
+                assert(item.data_type == DATA_PC);
+                ImGui::RadioButton("PLY", &item.save_type, SAVE_PLY);
+            }
 
             ImGui::TreePop();
         }

src/module/impl/image_saver_impl.h

@@ -11,16 +11,29 @@ struct image_saver::impl {
     enum {
         SAVE_JPG,
         SAVE_PNG,
-        SAVE_RAW
+        SAVE_RAW,
+        SAVE_PLY,
+    };
+
+    enum {
+        DATA_IMG,
+        DATA_PC
     };
 
     struct item_store_type {
         std::string ui_name;
         obj_name_type img_name;
         bool checked = false;
+        int data_type = DATA_IMG;
         int save_type = SAVE_PNG;
         impl *par = nullptr;
 
+        // for point cloud
+
+        void save_ply();
+
+        // for image
+
         void save_jpg();
 
         void save_png();