Implemented point cloud rendering.

CMakeLists.txt

@@ -16,6 +16,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/core/impl/image_utility_v2.cpp
         src/core/impl/memory_pool.cpp
         src/core/impl/object_manager.cpp
+        src/core/impl/pc_utility.cpp
         src/module_v3/registration.cpp
         src/module/impl/augment_manager.cpp
         src/module/impl/camera_augment_helper.cpp
@@ -25,6 +26,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/module/impl/image_saver.cpp
         src/module/impl/image_viewer.cpp
         src/module/impl/image_streamer.cpp
+        src/module/impl/pc_viewer.cpp
         src/network_v3/sender_tcp.cpp
         src/network_v3/sender_udp_fec.cpp
         #        src/network_v3/receiver_tcp.cpp
@@ -37,6 +39,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         #        src/network/impl/network.cpp
         src/render_v3/scene_render.cpp
         src/render/impl/render_mesh.cpp
+        src/render/impl/render_pc.cpp
         src/render/impl/render_texture.cpp
         src/render/impl/render_tools.cpp
         src/render/impl/render_utility.cpp)

src/core/cuda_helper.hpp

@@ -59,11 +59,11 @@ struct smart_cuda_stream {
 extern smart_cuda_stream *default_cuda_stream;
 
 inline cudaStream_t cuda_stream(smart_cuda_stream *stream = nullptr) {
-    return stream == nullptr ? nullptr : stream->cuda;
+    return (stream == nullptr) ? nullptr : stream->cuda;
 }
 
 inline cv::cuda::Stream &cv_stream(smart_cuda_stream *stream = nullptr) {
-    return stream == nullptr ? cv::cuda::Stream::Null() : stream->cv;
+    return (stream == nullptr) ? cv::cuda::Stream::Null() : stream->cv;
 }
 
 template<typename T>

src/core/image_utility_v2.h

@@ -4,6 +4,7 @@
 #include "core/cuda_helper.hpp"
 #include "core/image_utility.hpp"
 #include "core/memory_pool.h"
+#include "core/meta_helper.hpp"
 #include "image_process/cuda_impl/image_utility.cuh"
 
 #include <opencv2/core/types.hpp>
@@ -32,7 +33,8 @@ struct image_memory {
 };
 
 // collection of a same image in multiple devices
-class generic_image : public std::enable_shared_from_this<generic_image> {
+class generic_image : public meta_base,
+                      public std::enable_shared_from_this<generic_image> {
 public:
 
     struct create_config {
@@ -93,13 +95,6 @@ public:
 
     void cuda_modified(smart_cuda_stream *stream = nullptr);
 
-    using meta_key_type = size_t; // std::hash
-    using meta_value_type = uint64_t;
-
-    std::optional<meta_value_type> get_meta(meta_key_type key);
-
-    void set_meta(meta_key_type key, meta_value_type val);
-
 private:
     struct impl;
     std::unique_ptr<impl> pimpl;
@@ -107,6 +102,10 @@ private:
     pointer shallow_clone() const;
 
     friend class image_memory;
+
+public:
+
+    explicit generic_image(std::unique_ptr<impl> &&pimpl);
 };
 
 using image_ptr = generic_image::pointer;

src/core/impl/image_utility_v2.cpp

@@ -1,4 +1,6 @@
 #include "image_utility_v2_impl.h"
+#include "core/image_utility_v2.h"
+
 
 namespace image_utility_impl {
 
@@ -255,27 +257,14 @@ void generic_image::impl::cuda_modified(smart_cuda_stream *stream) {
     REC_CREATE(store_cuda.ptr, stream);
 }
 
-std::optional<generic_image::meta_value_type>
-generic_image::impl::get_meta(meta_key_type key) {
-    if (auto iter = meta_pool->find(key); iter != meta_pool->end()) {
-        return iter->second;
-    } else {
-        return {};
-    }
-}
-
-void generic_image::impl::set_meta(meta_key_type key, meta_value_type val) {
-    if (auto iter = meta_pool->find(key); iter != meta_pool->end()) {
-        iter->second = val;
-    } else {
-        meta_pool->emplace(key, val);
-    }
+generic_image::generic_image(std::unique_ptr<impl> &&_pimpl)
+        : meta_base(_pimpl.get()), pimpl(std::move(_pimpl)) {
+    pimpl->q_this = this;
 }
 
 generic_image::pointer generic_image::create(create_config conf) {
-    auto ret = std::make_shared<generic_image>();
-    ret->pimpl = std::make_unique<impl>(conf);
-    ret->pimpl->q_this = ret.get();
+    auto pimpl = std::make_unique<impl>(conf);
+    auto ret = std::make_shared<generic_image>(std::move(pimpl));
     return ret;
 }
 
@@ -348,9 +337,8 @@ template<> std::shared_ptr<smart_image<float1>> generic_image::v1() const;
 // @formatter:on
 
 generic_image::pointer generic_image::shallow_clone() const {
-    auto ret = std::make_shared<generic_image>();
-    ret->pimpl = std::make_unique<impl>(*pimpl);
-    ret->pimpl->q_this = ret.get();
+    auto pimpl_c = std::make_unique<impl>(*pimpl);
+    auto ret = std::make_shared<generic_image>(std::move(pimpl_c));
     return ret;
 }
 
@@ -372,13 +360,4 @@ void generic_image::host_modified(smart_cuda_stream *stream) {
 
 void generic_image::cuda_modified(smart_cuda_stream *stream) {
     pimpl->cuda_modified(stream);
-}
-
-std::optional<generic_image::meta_value_type>
-generic_image::get_meta(meta_key_type key) {
-    return pimpl->get_meta(key);
-}
-
-void generic_image::set_meta(meta_key_type key, meta_value_type val) {
-    pimpl->set_meta(key, val);
 }

src/core/impl/image_utility_v2_impl.h

@@ -24,17 +24,10 @@ namespace image_utility_impl {
 
 using namespace image_utility_impl;
 
-struct generic_image::impl {
+struct generic_image::impl : public meta_base::impl {
 
     generic_image *q_this = nullptr;
 
-    static constexpr auto max_meta_count = 16;
-    using meta_pool_type =
-            boost::container::flat_map<meta_key_type, meta_value_type, std::less<>,
-                    boost::container::static_vector<std::pair<meta_key_type, meta_value_type>, max_meta_count>>;
-    std::shared_ptr<meta_pool_type> meta_pool =
-            std::make_shared<meta_pool_type>();
-
     struct storage_info {
         std::shared_ptr<void> ptr;
         size_t pitch;
@@ -90,10 +83,6 @@ struct generic_image::impl {
 
     void cuda_modified(smart_cuda_stream *stream);
 
-    std::optional<meta_value_type> get_meta(meta_key_type key);
-
-    void set_meta(meta_key_type key, meta_value_type val);
-
 };
 
 #endif //DEPTHGUIDE_IMAGE_UTILITY_V2_IMPL_H

src/core/impl/memory_pool.cpp

@@ -93,7 +93,7 @@ void *memory_pool::impl::allocate_pitch(
         }
         case MEM_CUDA: {
             if (width & 0x1F) { // next multiples of 32
-                *pitch = (width + 0x20) & 0x1F;
+                *pitch = (width + 0x20) & ~0x1F;
             } else {
                 *pitch = width;
             }
@@ -139,6 +139,10 @@ void memory_pool::impl::deallocate(void *ptr) {
 }
 
 void memory_pool::impl::system_deallocate(mem_info_type mem_info) {
+    if (mem_info.event != nullptr) {
+        CUDA_API_CHECK(cudaEventDestroy(mem_info.event));
+    }
+
     switch (mem_info.loc) {
         case MEM_HOST: {
             ::free(mem_info.ptr);

src/core/impl/object_manager.cpp

@@ -21,6 +21,7 @@ object_manager::impl::query_st(name_type obj_name) {
 
 std::optional<object_manager::obj_info>
 object_manager::impl::query_info(name_type obj_name) {
+    assert(obj_name != invalid_obj_name);
     assert(switch_ctx() == nullptr);
     auto iter = obj_pool.find(obj_name);
     if (iter == obj_pool.end()) [[unlikely]] return {};

src/core/impl/pc_utility.cpp

@@ -0,0 +1,148 @@
+#include "pc_utility_impl.h"
+
+void *pc_memory::at(size_t idx) {
+    return (uint8_t *) start_ptr() + pc->elem_size() * idx;
+}
+
+void pc_memory::modified(smart_cuda_stream *stream) {
+    auto &pimpl = pc->pimpl;
+    if (ptr == pimpl->store_host.ptr) {
+        pimpl->q_this->host_modified(stream);
+    } else {
+        assert(ptr == pimpl->store_cuda.ptr);
+        pimpl->q_this->cuda_modified(stream);
+    }
+}
+
+void generic_pc::impl::storage_info::reset() {
+    ptr = nullptr;
+}
+
+generic_pc::impl::impl(create_config conf) {
+    size = conf.size;
+    fmt = conf.fmt;
+}
+
+size_t generic_pc::impl::elem_bytes() const {
+    switch (fmt) {
+        // @formatter:off
+        case PC_XYZ: { return sizeof(pc_xyz_type); }
+        case PC_XYZ_RGB: { return sizeof(pc_xyz_rgb_type); }
+        // @formatter:on
+        default: {
+            RET_ERROR_E;
+        }
+    }
+}
+
+size_t generic_pc::impl::size_in_bytes() const {
+    return elem_bytes() * size;
+}
+
+void generic_pc::impl::create_host(smart_cuda_stream *stream) {
+    if (store_host.ptr != nullptr) {
+        SYNC_CREATE(store_host.ptr, stream);
+        return;
+    }
+    store_host.ptr = ALLOC_SHARED(uint8_t, size_in_bytes(), MEM_HOST);
+    if (store_cuda.ptr != nullptr) {
+        SYNC_CREATE(store_cuda.ptr, stream);
+        CUDA_API_CHECK(cudaMemcpyAsync(store_host.ptr.get(), store_cuda.ptr.get(),
+                                       size_in_bytes(), cudaMemcpyDeviceToHost, cuda_stream(stream)));
+        REC_CREATE(store_host.ptr, stream);
+    }
+}
+
+void generic_pc::impl::create_cuda(smart_cuda_stream *stream) {
+    if (store_cuda.ptr != nullptr) {
+        SYNC_CREATE(store_cuda.ptr, stream);
+        return;
+    }
+    store_cuda.ptr = ALLOC_SHARED(uint8_t, size_in_bytes(), MEM_CUDA);
+    if (store_host.ptr != nullptr) {
+        SYNC_CREATE(store_host.ptr, stream);
+        CUDA_API_CHECK(cudaMemcpyAsync(store_cuda.ptr.get(), store_host.ptr.get(),
+                                       size_in_bytes(), cudaMemcpyHostToDevice, cuda_stream(stream)));
+        REC_CREATE(store_cuda.ptr, stream);
+    }
+}
+
+pc_memory generic_pc::impl::get_memory(memory_location loc,
+                                       smart_cuda_stream *stream) {
+    auto ret = pc_memory();
+    ret.pc = q_this->shared_from_this();
+
+    switch (loc) {
+        case MEM_HOST: {
+            create_host(stream);
+            ret.ptr = store_host.ptr;
+            break;
+        }
+        case MEM_CUDA: {
+            create_cuda(stream);
+            ret.ptr = store_cuda.ptr;
+            break;
+        }
+        default: {
+            RET_ERROR_E;
+        }
+    }
+    return ret;
+}
+
+void generic_pc::impl::host_modified(smart_cuda_stream *stream) {
+    assert(store_host.ptr != nullptr);
+    store_cuda.reset();
+    REC_CREATE(store_host.ptr, stream);
+}
+
+void generic_pc::impl::cuda_modified(smart_cuda_stream *stream) {
+    assert(store_cuda.ptr != nullptr);
+    store_host.reset();
+    REC_CREATE(store_cuda.ptr, stream);
+}
+
+generic_pc::generic_pc(std::unique_ptr<impl> &&_pimpl)
+        : meta_base(_pimpl.get()), pimpl(std::move(_pimpl)) {
+    pimpl->q_this = this;
+}
+
+generic_pc::pointer generic_pc::create(generic_pc::create_config conf) {
+    auto pimpl = std::make_unique<impl>(conf);
+    auto ret = std::make_shared<generic_pc>(std::move(pimpl));
+    return ret;
+}
+
+generic_pc::pointer generic_pc::create(size_t size, pc_format_enum fmt) {
+    auto conf = create_config{.size = size, .fmt =fmt};
+    return create(conf);
+}
+
+size_t generic_pc::size() const {
+    return pimpl->size;
+}
+
+size_t generic_pc::elem_size() const {
+    return pimpl->elem_bytes();
+}
+
+void generic_pc::shrink(size_t size) {
+    assert(size <= pimpl->size);
+    pimpl->size = size;
+}
+
+pc_format_enum generic_pc::format() const {
+    return pimpl->fmt;
+}
+
+pc_memory generic_pc::memory(memory_location loc, smart_cuda_stream *stream) {
+    return pimpl->get_memory(loc, stream);
+}
+
+void generic_pc::host_modified(smart_cuda_stream *stream) {
+    pimpl->host_modified(stream);
+}
+
+void generic_pc::cuda_modified(smart_cuda_stream *stream) {
+    pimpl->cuda_modified(stream);
+}

src/core/impl/pc_utility_impl.h

@@ -0,0 +1,43 @@
+#ifndef DEPTHGUIDE_PC_UTILITY_IMPL_H
+#define DEPTHGUIDE_PC_UTILITY_IMPL_H
+
+#include "core/pc_utility.h"
+
+struct generic_pc::impl : public meta_base::impl {
+
+    generic_pc *q_this = nullptr;
+
+    struct storage_info {
+        std::shared_ptr<void> ptr;
+
+        void reset();
+    };
+
+    storage_info store_host;
+    storage_info store_cuda;
+
+    size_t size;
+    pc_format_enum fmt;
+
+    explicit impl(create_config conf);
+
+    size_t elem_bytes() const;
+
+    size_t size_in_bytes() const;
+
+    // imply that stream want to use host
+    void create_host(smart_cuda_stream *stream);
+
+    // imply that stream want to use cuda
+    void create_cuda(smart_cuda_stream *stream);
+
+    pc_memory get_memory(memory_location loc,
+                         smart_cuda_stream *stream);
+
+    void host_modified(smart_cuda_stream *stream);
+
+    void cuda_modified(smart_cuda_stream *stream);
+
+};
+
+#endif //DEPTHGUIDE_PC_UTILITY_IMPL_H

src/core/math_helper.hpp

@@ -7,6 +7,7 @@
 
 #include <Eigen/Geometry>
 
+// r in radius
 inline glm::mat4 to_transform_mat(glm::vec3 t, glm::vec3 r) {
     static constexpr auto unit_x = glm::vec3(1.0f, 0.0f, 0.0f);
     static constexpr auto unit_y = glm::vec3(0.0f, 1.0f, 0.0f);

src/core/meta_helper.hpp

@@ -0,0 +1,64 @@
+#ifndef DEPTHGUIDE_META_HELPER_HPP
+#define DEPTHGUIDE_META_HELPER_HPP
+
+#include <boost/container/flat_map.hpp>
+#include <boost/container/static_vector.hpp>
+
+#include <optional>
+
+class meta_base {
+public:
+
+    virtual ~meta_base() = default;
+
+    using meta_key_type = size_t; // std::hash
+    using meta_value_type = uint64_t;
+
+    std::optional<meta_value_type> get_meta(meta_key_type key) {
+        return pimpl->get_meta(key);
+    }
+
+    void set_meta(meta_key_type key, meta_value_type val) {
+        return pimpl->set_meta(key, val);
+    }
+
+protected:
+
+    struct impl {
+        static constexpr auto max_meta_count = 16;
+        using meta_pool_type =
+                boost::container::flat_map<meta_key_type, meta_value_type, std::less<>,
+                        boost::container::static_vector<std::pair<meta_key_type, meta_value_type>, max_meta_count>>;
+        std::shared_ptr<meta_pool_type> meta_pool =
+                std::make_shared<meta_pool_type>();
+
+        std::optional<meta_value_type> get_meta(meta_key_type key) {
+            if (auto iter = meta_pool->find(key); iter != meta_pool->end()) {
+                return iter->second;
+            } else {
+                return {};
+            }
+        }
+
+        void set_meta(meta_key_type key, meta_value_type val) {
+            if (auto iter = meta_pool->find(key); iter != meta_pool->end()) {
+                iter->second = val;
+            } else {
+                meta_pool->emplace(key, val);
+            }
+        }
+
+    };
+
+private:
+    impl *pimpl = nullptr; // use child's pimpl
+
+public:
+
+    explicit meta_base(impl *_pimpl) {
+        pimpl = _pimpl;
+    }
+
+};
+
+#endif //DEPTHGUIDE_META_HELPER_HPP

src/core/pc_utility.h

@@ -0,0 +1,92 @@
+#ifndef DEPTHGUIDE_PC_UTILITY_H
+#define DEPTHGUIDE_PC_UTILITY_H
+
+#include "core/cuda_helper.hpp"
+#include "core/memory_pool.h"
+#include "core/meta_helper.hpp"
+
+#include <memory>
+
+enum pc_format_enum {
+    PC_XYZ,
+    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 {
+    std::shared_ptr<generic_pc> pc;
+    std::shared_ptr<void> ptr;
+
+    void *start_ptr() const {
+        return ptr.get();
+    }
+
+    // pointer to ith point
+    void *at(size_t idx);
+
+    void modified(smart_cuda_stream *stream = nullptr);
+};
+
+class generic_pc : public meta_base,
+                   public std::enable_shared_from_this<generic_pc> {
+public:
+
+    struct create_config {
+        size_t size;
+        pc_format_enum fmt;
+    };
+
+    using pointer = std::shared_ptr<generic_pc>;
+
+    static pointer create(create_config conf);
+
+    static pointer create(size_t size, pc_format_enum fmt);
+
+    // use to hide invisible points
+    void shrink(size_t size);
+
+    size_t size() const; // number of points
+
+    size_t elem_size() const; // byte of each point
+
+    size_t size_in_bytes() const {
+        return size() * elem_size();
+    }
+
+    pc_format_enum format() const;
+
+    pc_memory memory(memory_location loc,
+                     smart_cuda_stream *stream = nullptr);
+
+    void host_modified(smart_cuda_stream *stream = nullptr);
+
+    void cuda_modified(smart_cuda_stream *stream = nullptr);
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+
+    friend class pc_memory;
+
+public:
+    explicit generic_pc(std::unique_ptr<impl> &&pimpl);
+
+};
+
+using pc_ptr = generic_pc::pointer;
+
+inline pc_ptr create_pc(size_t size, pc_format_enum fmt) {
+    return generic_pc::create(size, fmt);
+}
+
+#endif //DEPTHGUIDE_PC_UTILITY_H

src/device/impl/orb_camera.cpp

@@ -61,6 +61,29 @@ namespace orb_camera_impl {
         return create_image(f32_info);
     }
 
+    pc_ptr pc_frame_to_pc(const frame_type &frame) {
+        assert(frame->format() == OB_FORMAT_RGB_POINT);
+        auto size = frame->dataSize() / sizeof(OBColorPoint);
+        auto pc = create_pc(size, PC_XYZ_RGB);
+        auto pc_mem = pc->memory(MEM_HOST);
+        auto start_ptr = (pc_xyz_rgb_type *) pc_mem.start_ptr();
+        auto ptr = start_ptr;
+        for (auto k = 0; k < size; ++k) { // TODO: do conversion in CUDA
+            auto ob_ptr = (OBColorPoint *) frame->data() + k;
+            if (ob_ptr->z == 0) continue;
+            ptr->x = ob_ptr->x;
+            ptr->y = ob_ptr->y;
+            ptr->z = ob_ptr->z;
+            ptr->r = ob_ptr->r;
+            ptr->g = ob_ptr->g;
+            ptr->b = ob_ptr->b;
+            ++ptr;
+        }
+        pc->shrink(ptr - start_ptr);
+        pc_mem.modified();
+        return pc;
+    }
+
 }
 
 std::shared_ptr<ob::Device> orb_camera::impl::get_device(const char *sn) {
@@ -74,6 +97,11 @@ orb_camera::impl *orb_camera::impl::create(orb_camera::create_config conf) {
     ret->pipe = std::make_shared<ob::Pipeline>(dev);
     ret->ctx = conf.ctx;
     ret->stream = conf.stream;
+
+    // load profiles
+    ret->c_pf_list = ret->pipe->getStreamProfileList(OB_SENSOR_COLOR);
+    ret->d_pf_list = ret->pipe->getStreamProfileList(OB_SENSOR_DEPTH);
+
     return ret;
 }
 
@@ -83,9 +111,9 @@ orb_camera::impl::~impl() {
     }
 }
 
-void orb_camera::impl::create_video_info_list(
-        const pf_list_type &pf_list, v_info_list_type *v_info) {
-    v_info->reserve(pf_list->count());
+orb_camera::vi_list_ptr orb_camera::impl::create_video_info_list(const pf_list_type &pf_list) {
+    auto ret = std::make_shared<vi_list_ptr::element_type>();
+    ret->reserve(pf_list->count());
     for (uint32_t k = 0; k < pf_list->count(); ++k) {
         auto pf = pf_list->getProfile(k)->as<ob::VideoStreamProfile>();
         assert(pf != nullptr);
@@ -95,56 +123,79 @@ void orb_camera::impl::create_video_info_list(
         info.fps = pf->fps();
         info.width = pf->width();
         info.height = pf->height();
-        v_info->push_back(info);
+        ret->push_back(info);
     }
+    return ret;
 }
 
-orb_camera::impl::v_info_list_type *
-orb_camera::impl::query_video_info(OBSensorType s_type) {
+orb_camera::vi_list_ptr orb_camera::impl::query_video_info(OBSensorType s_type) {
     switch (s_type) {
         case OB_SENSOR_COLOR: {
-            if (c_pf_list == nullptr) [[unlikely]] {
-                c_pf_list = pipe->getStreamProfileList(s_type);
-                c_info = std::make_unique<v_info_list_type>();
-                create_video_info_list(c_pf_list, c_info.get());
+            if (c_info == nullptr) [[unlikely]] {
+                c_info = create_video_info_list(c_pf_list);
             }
-            return c_info.get();
+            return c_info;
         }
         case OB_SENSOR_DEPTH: {
-            if (d_pf_list == nullptr) [[unlikely]] {
-                d_pf_list = pipe->getStreamProfileList(s_type);
-                d_info = std::make_unique<v_info_list_type>();
-                create_video_info_list(d_pf_list, d_info.get());
+            if (d_info == nullptr) [[unlikely]] {
+                d_info = create_video_info_list(d_pf_list);
             }
-            return d_info.get();
+            return d_info;
+        }
+        default: {
+            RET_ERROR_E;
         }
     }
-    RET_ERROR_P;
+}
+
+orb_camera::vi_list_ptr orb_camera::impl::query_d2c_info(uint32_t c_conf_index) {
+    auto c_pf = c_pf_list->getProfile(c_conf_index);
+    auto d_pf = pipe->getD2CDepthProfileList(c_pf, align_mode);
+    assert(d_pf->count() > 0);
+    return create_video_info_list(d_pf);
 }
 
 bool orb_camera::impl::start(start_config conf) {
     assert(!is_capturing);
     auto ob_conf = std::make_shared<ob::Config>();
 
+    auto c_prof = std::shared_ptr<ob::StreamProfile>();
     if (conf.color.enable) {
         assert(c_pf_list != nullptr);
-        ob_conf->enableStream(c_pf_list->getProfile(conf.color.config_index));
+        c_prof = c_pf_list->getProfile(conf.color.config_index);
+        ob_conf->enableStream(c_prof);
         c_name = conf.color.name;
     }
+
+    auto d_prof = std::shared_ptr<ob::StreamProfile>();
     if (conf.depth.enable) {
         assert(d_pf_list != nullptr);
-        ob_conf->enableStream(d_pf_list->getProfile(conf.depth.config_index));
+        if (conf.pc.enable) { // TODO: better determine method
+            assert(c_prof != nullptr);
+            d_prof = pipe->getD2CDepthProfileList(c_prof, align_mode)->getProfile(conf.depth.config_index);
+        } else {
+            d_prof = d_pf_list->getProfile(conf.depth.config_index);
+        }
+        ob_conf->enableStream(d_prof);
         d_name = conf.depth.name;
     }
 
     if (conf.color.enable && conf.depth.enable) {
-        ob_conf->setAlignMode(ALIGN_D2C_HW_MODE);
+        ob_conf->setAlignMode(align_mode);
     }
 
-    pipe->start(ob_conf, [this](auto frames) {
-        frames_callback(std::move(frames));
-    });
+    pipe->start(ob_conf, [this](auto frames) { frames_callback(frames); });
     is_capturing = true;
+
+    // Example says that the filter depends on the started pipeline.
+    if (conf.pc.enable) {
+        pc_filter = std::make_shared<ob::PointCloudFilter>();
+        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;
+    }
+
     return true;
 }
 
@@ -169,16 +220,28 @@ void orb_camera::impl::frames_callback(const frames_type &frames) {
         d_img = depth_y16_to_mm(y16_img, d_frame->getValueScale());
     }
 
+    if (pc_filter != nullptr &&
+        c_img != nullptr && d_img != nullptr) { // depth images may not be ready
+        pc_filter->setPositionDataScaled(frames->depthFrame()->getValueScale());
+        pc_filter->pushFrame(frames);
+    }
+
     post(*ctx, [=, _c_name = c_name, _d_name = d_name] {
         if (c_img != nullptr) { OBJ_SAVE(_c_name, c_img); }
         if (d_img != nullptr) { OBJ_SAVE(_d_name, d_img); }
     });
 }
 
+void orb_camera::impl::pc_callback(const frame_type &frame) {
+    auto pc = pc_frame_to_pc(frame);
+    OBJ_SAVE(pc_name, pc);
+}
+
 void orb_camera::impl::stop() {
     assert(is_capturing);
     pipe->stop();
     is_capturing = false;
+    pc_filter = nullptr;
 }
 
 orb_camera::pointer orb_camera::create(create_config conf) {
@@ -202,8 +265,7 @@ orb_camera::query_device_info() {
     return ret;
 }
 
-std::vector<orb_camera::video_info> *
-orb_camera::query_video_info(video_type type) {
+orb_camera::vi_list_ptr orb_camera::query_video_info(video_type type) {
     switch (type) {
         case V_COLOR:
             return pimpl->query_video_info(OB_SENSOR_COLOR);
@@ -213,6 +275,10 @@ orb_camera::query_video_info(video_type type) {
     RET_ERROR_P;
 }
 
+orb_camera::vi_list_ptr orb_camera::query_d2c_info(uint32_t c_conf_index) {
+    return pimpl->query_d2c_info(c_conf_index);
+}
+
 bool orb_camera::start(orb_camera::start_config conf) {
     return pimpl->start(conf);
 }

src/device/impl/orb_camera_impl.h

@@ -3,6 +3,7 @@
 
 #include "device/orb_camera.h"
 #include "core/image_utility.hpp"
+#include "core/pc_utility.h"
 
 #include <boost/asio/io_context.hpp>
 
@@ -14,6 +15,7 @@ namespace orb_camera_impl {
 
     const char *get_fmt_name(OBFormat fmt);
 
+    using frame_type = std::shared_ptr<ob::Frame>;
     using video_frame_type = std::shared_ptr<ob::VideoFrame>;
 //    using color_frame_type = std::shared_ptr<ob::ColorFrame>;
 //    using depth_frame_type = std::shared_ptr<ob::DepthFrame>;
@@ -21,6 +23,8 @@ namespace orb_camera_impl {
     // new_value (mm) = old_value * scale
     image_f32c1 depth_y16_to_mm(const image_u16c1 &y16, float scale);
 
+    pc_ptr pc_frame_to_pc(const frame_type &frame);
+
 }
 
 using boost::asio::io_context;
@@ -29,6 +33,7 @@ using namespace orb_camera_impl;
 struct orb_camera::impl {
 
     std::shared_ptr<ob::Pipeline> pipe;
+    std::shared_ptr<ob::PointCloudFilter> pc_filter;
     smart_cuda_stream *stream = nullptr;
     io_context *ctx = nullptr;
 
@@ -36,12 +41,14 @@ struct orb_camera::impl {
     pf_list_type c_pf_list; // color profile list
     pf_list_type d_pf_list; // depth profile list
 
-    using v_info_list_type = std::vector<video_info>;
-    std::unique_ptr<v_info_list_type> c_info;
-    std::unique_ptr<v_info_list_type> d_info;
+    vi_list_ptr c_info;
+    vi_list_ptr d_info;
 
     obj_name_type c_name = invalid_obj_name;
     obj_name_type d_name = invalid_obj_name;
+    obj_name_type pc_name = invalid_obj_name;
+
+    static constexpr auto align_mode = ALIGN_D2C_HW_MODE;
 
     bool is_capturing = false;
 
@@ -51,9 +58,11 @@ struct orb_camera::impl {
 
     static impl *create(create_config conf);
 
-    static void create_video_info_list(const pf_list_type &pf_list, v_info_list_type *v_info);
+    static vi_list_ptr create_video_info_list(const pf_list_type &pf_list);
+
+    vi_list_ptr query_video_info(OBSensorType s_type);
 
-    v_info_list_type *query_video_info(OBSensorType s_type);
+    vi_list_ptr query_d2c_info(uint32_t c_conf_index);
 
     bool start(start_config conf);
 
@@ -61,6 +70,10 @@ struct orb_camera::impl {
 
     void frames_callback(const frames_type &frames);
 
+    using frame_type = std::shared_ptr<ob::Frame>;
+
+    void pc_callback(const frame_type &frame);
+
     void stop();
 
 };

src/device/impl/orb_camera_ui.cpp

@@ -1,6 +1,7 @@
 #include "orb_camera_ui_impl.h"
 #include "core/image_utility.hpp"
 #include "core/imgui_utility.hpp"
+#include "core/pc_utility.h"
 
 #include <boost/asio/io_context.hpp>
 #include <boost/asio/post.hpp>
@@ -12,8 +13,12 @@ orb_camera_ui::impl::impl(create_config conf) {
     ctx = conf.ctx;
     cam_c_conf.stream = conf.stream;
     cam_c_conf.ctx = ctx;
+
+    cam_s_conf.color.enable = true;
     cam_s_conf.color.name = conf.cf_name;
+    cam_s_conf.depth.enable = true;
     cam_s_conf.depth.name = conf.df_name;
+    cam_s_conf.pc.name = conf.pc_name;
 
     refresh_dev_info_list();
 }
@@ -22,6 +27,24 @@ void orb_camera_ui::impl::refresh_dev_info_list() {
     dev_info_list = orb_camera::query_device_info();
 }
 
+void orb_camera_ui::impl::refresh_d_conf_list() {
+    d_conf_list.clear();
+    auto dv_info = orb_camera::vi_list_ptr();
+    if (!enable_pc) {
+        dv_info = cam->query_video_info(orb_camera::V_DEPTH);
+    } else {
+        dv_info = cam->query_d2c_info(c_conf_list[c_conf_index].index);
+    }
+    for (auto v_info: *dv_info) {
+        if (strcmp(v_info.fmt_name, "y16") == 0) {
+            d_conf_list.emplace_back(v_info.index, std::string(v_info));
+        }
+    }
+    if (d_conf_index >= d_conf_list.size()) {
+        d_conf_index = 0;
+    }
+}
+
 void orb_camera_ui::impl::open_camera() {
     cam_c_conf.sn_str = dev_info_list[dev_index].sn_str.c_str();
     cam = orb_camera::create(cam_c_conf);
@@ -36,19 +59,14 @@ void orb_camera_ui::impl::open_camera() {
         }
     }
 
-    d_conf_list.clear();
-    auto dv_info = cam->query_video_info(orb_camera::V_DEPTH);
-    for (auto v_info: *dv_info) {
-        if (strcmp(v_info.fmt_name, "y16") == 0) {
-            d_conf_list.emplace_back(v_info.index, std::string(v_info));
-        }
-    }
+    refresh_d_conf_list();
 }
 
 void orb_camera_ui::impl::start_camera() {
     assert(cam != nullptr);
     cam_s_conf.color.config_index = c_conf_list[c_conf_index].index;
     cam_s_conf.depth.config_index = d_conf_list[d_conf_index].index;
+    cam_s_conf.pc.enable = enable_pc;
     cam->start(cam_s_conf);
 }
 
@@ -90,6 +108,7 @@ void orb_camera_ui::impl::show_config() {
                 auto is_selected = (c_conf_index == k);
                 if (ImGui::Selectable(c_conf_list[k].dis_name.c_str(), is_selected)) {
                     c_conf_index = k;
+                    refresh_d_conf_list();
                 }
                 if (is_selected) {
                     ImGui::SetItemDefaultFocus();
@@ -112,6 +131,10 @@ void orb_camera_ui::impl::show_config() {
             ImGui::EndCombo();
         }
 
+        ImGui::SameLine();
+        if (ImGui::Checkbox("Point Cloud", &enable_pc)) {
+            refresh_d_conf_list();
+        }
     }
 }
 
@@ -156,6 +179,14 @@ void orb_camera_ui::impl::show() {
         auto size = d_img->size();
         ImGui::Text("Depth Stream: %dx%d / %.2fms", size.width, size.height, d_interval);
     }
+    if (enable_pc) {
+        auto pc = OBJ_QUERY(pc_ptr, cam_s_conf.pc.name);
+        auto interval = OBJ_STATS(cam_s_conf.pc.name).save_interval;
+        if (pc != nullptr) {
+            auto num = pc->size();
+            ImGui::Text("Point Cloud: %ld / %.2fms", num, interval);
+        }
+    }
 }
 
 orb_camera_ui::orb_camera_ui(create_config conf)

src/device/impl/orb_camera_ui_impl.h

@@ -28,10 +28,14 @@ struct orb_camera_ui::impl {
     orb_camera::start_config cam_s_conf;
     std::unique_ptr<orb_camera> cam;
 
+    bool enable_pc = true;
+
     explicit impl(create_config conf);
 
     void refresh_dev_info_list();
 
+    void refresh_d_conf_list();
+
     void open_camera();
 
     void start_camera();

src/device/orb_camera.h

@@ -46,7 +46,11 @@ public:
         V_COLOR, V_DEPTH
     };
 
-    std::vector<video_info> *query_video_info(video_type type);
+    using vi_list_ptr = std::shared_ptr<std::vector<video_info>>;
+
+    vi_list_ptr query_video_info(video_type type);
+
+    vi_list_ptr query_d2c_info(uint32_t c_conf_index); // color config index
 
     struct start_config {
         struct color_config_type {
@@ -59,6 +63,10 @@ public:
             uint32_t config_index = 0;
             obj_name_type name = -1;
         } depth;
+        struct pc_config_type { // point cloud
+            bool enable = false;
+            obj_name_type name = -1;
+        } pc;
     };
 
     bool start(start_config conf);

src/device/orb_camera_ui.h

@@ -12,6 +12,7 @@ public:
     struct create_config {
         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
         boost::asio::io_context *ctx = nullptr;
         smart_cuda_stream *stream = default_cuda_stream;
     };

src/impl/apps/depth_guide/depth_guide.cpp

@@ -1,6 +1,7 @@
 #include "depth_guide.h"
 #include "core/image_utility.hpp"
 #include "core/imgui_utility.hpp"
+#include "core/pc_utility.h"
 #include "image_process/impl/versatile_convertor_impl.h"
 
 app_depth_guide::app_depth_guide(const create_config &_conf) {
@@ -13,10 +14,11 @@ app_depth_guide::app_depth_guide(const create_config &_conf) {
     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(pc_raw, pc_ptr());
 
     // initialize modules
     auto orb_cam_conf = orb_camera_ui::create_config{
-            .cf_name = img_color, .df_name = img_depth,
+            .cf_name = img_color, .df_name = img_depth, .pc_name = pc_raw,
             .ctx = conf.asio_ctx, .stream = default_cuda_stream,
     };
     orb_cam = std::make_unique<orb_camera_ui>(orb_cam_conf);
@@ -50,6 +52,11 @@ app_depth_guide::app_depth_guide(const create_config &_conf) {
             .cuda_ctx = conf.cuda_ctx, .stream = default_cuda_stream
     };
     out_streamer = std::make_unique<image_streamer>(out_streamer_conf);
+
+    auto pc_conf = pc_viewer::create_config{
+            .name = pc_raw, .stream = default_cuda_stream,
+    };
+    scene_viewer = std::make_unique<pc_viewer>(pc_conf);
 }
 
 void app_depth_guide::show_ui() {
@@ -69,7 +76,8 @@ void app_depth_guide::show_ui() {
 
         if (ImGui::CollapsingHeader("Debug")) {
             if (ImGui::TreeNode("Background")) {
-                bg_viewer->show();
+//                bg_viewer->show();
+                scene_viewer->show();
                 ImGui::TreePop();
             }
             if (ImGui::TreeNode("Memory Pool")) {
@@ -91,5 +99,6 @@ void app_depth_guide::show_ui() {
 }
 
 void app_depth_guide::render_background() {
-    bg_viewer->render();
+//    bg_viewer->render();
+    scene_viewer->render();
 }

src/impl/apps/depth_guide/depth_guide.h

@@ -8,6 +8,7 @@
 #include "module/image_augment_helper.h"
 #include "module/image_streamer.h"
 #include "module/image_viewer.h"
+#include "module/pc_viewer.h"
 #include "impl/app_base.h"
 
 #include <boost/asio/io_context.hpp>
@@ -37,6 +38,9 @@ private:
 
         // output image
         img_out,
+
+        // point cloud from device
+        pc_raw,
     };
 
     create_config conf;
@@ -47,6 +51,7 @@ private:
     std::unique_ptr<versatile_convertor> depth_encode;
     std::unique_ptr<stereo_augment_helper> out_combiner;
     std::unique_ptr<image_streamer> out_streamer; // output streamer
+    std::unique_ptr<pc_viewer> scene_viewer;
 
     // miscellaneous
     event_timer perf_timer; // performance timer

src/module/impl/pc_viewer.cpp

@@ -0,0 +1,54 @@
+#include "pc_viewer_impl.h"
+#include "core/imgui_utility.hpp"
+#include "core/math_helper.hpp"
+
+#include <glm/gtc/type_ptr.hpp>
+
+pc_viewer::impl::impl(pc_viewer::create_config _conf) {
+    conf = _conf;
+
+    // load point size information
+    glGetFloatv(GL_POINT_SIZE_RANGE, (GLfloat *) &ps_range);
+    glGetFloatv(GL_POINT_SIZE_GRANULARITY, &ps_interval);
+}
+
+void pc_viewer::impl::render() {
+    auto r_conf = pc_render::config_type();
+    r_conf.pc.point_size = point_size;
+    // camera coordinate -> view space coordinate
+    auto real_rot = rotation + glm::vec3(180.f, 0.f, 0.f);
+    r_conf.camera.transform = to_transform_mat(offset, glm::radians(real_rot));
+    r_conf.camera.fov = fov;
+    r_conf.camera.near = clip.near;
+    r_conf.camera.far = clip.far;
+    r_conf.stream = conf.stream;
+    ren.render(conf.name, r_conf);
+}
+
+void pc_viewer::impl::show() {
+    ImGui::DragFloat3("Offset (mm)", glm::value_ptr(offset),
+                      0.05f, 0.0f, 0.0f, "%.02f");
+    ImGui::DragFloat3("Offset (deg)", glm::value_ptr(rotation),
+                      0.1f, -180.0f, 180.0f, "%.01f");
+    ImGui::DragFloat("FOV (deg)", &fov, 0.5f, 10.0f, 178.0f, "%.01f");
+    ImGui::DragFloat("Point Size", &point_size, ps_interval,
+                     ps_range.min, ps_range.max, "%.01f");
+    ImGui::SliderFloat("Clip Near", &clip.near,
+                       1.0f, clip.far, "%.f", ImGuiSliderFlags_Logarithmic);
+    ImGui::SliderFloat("Clip Far", &clip.far,
+                       clip.near, 10000.0f, "%.f", ImGuiSliderFlags_Logarithmic);
+}
+
+pc_viewer::pc_viewer(create_config conf)
+        : pimpl(std::make_unique<impl>(conf)) {
+}
+
+pc_viewer::~pc_viewer() = default;
+
+void pc_viewer::show() {
+    pimpl->show();
+}
+
+void pc_viewer::render() {
+    pimpl->render();
+}

src/module/impl/pc_viewer_impl.h

@@ -0,0 +1,38 @@
+#ifndef DEPTHGUIDE_PC_VIEWER_HPP
+#define DEPTHGUIDE_PC_VIEWER_HPP
+
+#include "module/pc_viewer.h"
+#include "render/render_tools.h"
+
+#include <glm/glm.hpp>
+
+struct pc_viewer::impl {
+
+    create_config conf;
+
+    glm::vec3 offset = {};
+    glm::vec3 rotation = {};
+    float fov = 60.0f;
+
+    struct {
+        float near = 0.1f;
+        float far = 1000.0f;
+    } clip;
+
+    struct {
+        GLfloat min, max;
+    } ps_range;
+    GLfloat ps_interval;
+    float point_size = 1.0f;
+
+    pc_render ren;
+
+    explicit impl(create_config conf);
+
+    void show();
+
+    void render();
+
+};
+
+#endif //DEPTHGUIDE_PC_VIEWER_HPP

src/module/pc_viewer.h

@@ -0,0 +1,30 @@
+#ifndef DEPTHGUIDE_PC_VIEWER_H
+#define DEPTHGUIDE_PC_VIEWER_H
+
+#include "core/cuda_helper.hpp"
+#include "core/object_manager.h"
+
+#include <memory>
+
+class pc_viewer {
+public:
+
+    struct create_config {
+        obj_name_type name;
+        smart_cuda_stream *stream;
+    };
+
+    explicit pc_viewer(create_config conf);
+
+    ~pc_viewer();
+
+    void show();
+
+    void render();
+
+private:
+    struct impl;
+    std::unique_ptr<impl> pimpl;
+};
+
+#endif //DEPTHGUIDE_PC_VIEWER_H

src/render/impl/render_pc.cpp

@@ -0,0 +1,55 @@
+#include "render_pc_impl.h"
+
+#include <cstddef>
+
+namespace render_pc_impl {
+
+//    GLuint vao_xyz = 0;
+//    GLuint vao_rgb = 0;
+
+    using pg_type = std::unique_ptr<smart_program>;
+    pg_type pg_xyz; // render PC_XYZ
+    pg_type pg_rgb; // render PC_XYZ_RGB
+
+    void render_pc_rgb(const pc_render_info &info) {
+        // initialize
+        if (pg_rgb == nullptr) [[unlikely]] {
+            pg_rgb = std::unique_ptr<smart_program>(
+                    smart_program::create("pc_rgb",
+                                          {{GL_VERTEX_SHADER,   "pc_rgb.vert"},
+                                           {GL_FRAGMENT_SHADER, "pc_rgb.frag"}}));
+        }
+
+        assert(pg_rgb != nullptr);
+        pg_rgb->use();
+
+        // mesh info
+        pg_rgb->set_uniform_mat4("transform", info.camera.project
+                                              * info.camera.transform
+                                              * info.pc.transform);
+        pg_rgb->set_uniform_f("point_size", info.point_size);
+
+        glEnable(GL_DEPTH_TEST);
+        glEnable(GL_PROGRAM_POINT_SIZE);
+
+        glBindVertexArray(info.pc.vao);
+        glBindBuffer(GL_ARRAY_BUFFER, info.pc.vbo);
+
+        glDrawArrays(GL_POINTS, 0, info.pc.num);
+    }
+
+}
+
+using namespace render_pc_impl;
+
+void render_pc(const pc_render_info &info) {
+    switch (info.pc.format) {
+        case PC_XYZ_RGB: {
+            render_pc_rgb(info);
+            break;
+        }
+        default: {
+            RET_ERROR;
+        }
+    }
+}

src/render/impl/render_pc_impl.h

@@ -0,0 +1,14 @@
+#ifndef DEPTHGUIDE_RENDER_PC_IMP_H
+#define DEPTHGUIDE_RENDER_PC_IMP_H
+
+#include "render/render_pc.h"
+
+namespace render_pc_impl {
+
+    void render_pc_xyz(const pc_render_info &info);
+
+    void render_pc_rgb(const pc_render_info &info);
+
+}
+
+#endif //DEPTHGUIDE_RENDER_PC_IMP_H

src/render/impl/render_tools.cpp

@@ -1,8 +1,11 @@
 #include "render/render_texture.h"
 #include "render/render_tools.h"
+#include "render/render_pc.h"
 
 #include <opencv2/imgproc.hpp>
 
+#include <glm/gtx/transform.hpp>
+
 simple_rect calc_render_range(cv::Size img_size) {
     return simple_rect{-1, -1, 2, 2}
             .fit_aspect(img_size.aspectRatio() /
@@ -125,6 +128,59 @@ void depth_image_render::render(obj_name_type name, config_type conf) {
     color_render.render_rgba(img_color, c_conf);
 }
 
+void pc_render::render(obj_name_type name, const pc_render::config_type &conf) {
+    auto pc = OBJ_QUERY(pc_ptr, name);
+    if (pc == nullptr) [[unlikely]] return;
+    vbo.upload(pc, conf.stream);
+
+    // create vao if needed
+    if (vao == 0
+        || last_vbo != vbo.id
+        || last_fmt != pc->format()) {
+
+        last_vbo = vbo.id;
+        last_fmt = pc->format();
+
+        glBindBuffer(GL_ARRAY_BUFFER, last_vbo);
+
+        if (vao != 0) {
+            glDeleteVertexArrays(1, &vao);
+        }
+        glGenVertexArrays(1, &vao);
+        glBindVertexArray(vao);
+
+        switch (last_fmt) {
+            case PC_XYZ_RGB: {
+                glEnableVertexAttribArray(0);
+                glEnableVertexAttribArray(1);
+                glVertexAttribPointer(0, 3, GL_FLOAT, false,
+                                      sizeof(pc_xyz_rgb_type), (void *) offsetof(pc_xyz_rgb_type, x));
+                glVertexAttribPointer(1, 3, GL_UNSIGNED_BYTE, true,
+                                      sizeof(pc_xyz_rgb_type), (void *) offsetof(pc_xyz_rgb_type, r));
+                break;
+            }
+            default: {
+                RET_ERROR;
+            }
+        }
+    }
+    assert(vao != 0);
+
+    auto ren_info = pc_render_info();
+    ren_info.pc.format = pc->format();
+    ren_info.pc.vao = vao;
+    ren_info.pc.vbo = vbo.id;
+    ren_info.pc.num = pc->size();
+    ren_info.pc.transform = conf.pc.transform;
+    ren_info.pc.color = conf.pc.color;
+    ren_info.camera.transform = conf.camera.transform;
+    ren_info.point_size = conf.pc.point_size;
+    float aspect = query_viewport_size().aspectRatio();
+    ren_info.camera.project = glm::perspective(
+            glm::radians(conf.camera.fov), aspect, conf.camera.near, conf.camera.far);
+    render_pc(ren_info);
+}
+
 //frame_builder::frame_builder(const create_config &_conf) {
 //    conf = _conf;
 //    fbo.create(conf.fbo_conf);

src/render/impl/render_utility.cpp

@@ -176,21 +176,28 @@ void smart_pixel_buffer::deallocate() {
         CUDA_API_CHECK(cudaGraphicsUnregisterResource(cuda_res_down));
         cuda_res_down = nullptr;
     }
-
-    img_ptr = nullptr;
 }
 
 void smart_pixel_buffer::create(GLsizeiptr _size) {
     if (_size == size) [[likely]] return;
+    if (_size <= capacity) { // prevent frequent allocation
+        size = _size;
+        return;
+    }
+
+    // allocate before deallocate to prevent the error
+    // that same id may actually point to different OpenGL object
+    GLuint next_id = 0;
+    glGenBuffers(1, &next_id);
+    glBindBuffer(GL_PIXEL_PACK_BUFFER, next_id);
+    glBufferStorage(GL_PIXEL_PACK_BUFFER, _size, nullptr, GL_DYNAMIC_STORAGE_BIT);
+    glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
 
     deallocate();
 
-    // allocate
+    id = next_id;
     size = _size;
-    glGenBuffers(1, &id);
-    glBindBuffer(GL_PIXEL_PACK_BUFFER, id);
-    glBufferStorage(GL_PIXEL_PACK_BUFFER, size, nullptr, GL_DYNAMIC_STORAGE_BIT);
-    glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
+    capacity = size;
 }
 
 void smart_pixel_buffer::download_viewport(GLenum format, GLenum type) {
@@ -232,24 +239,56 @@ void smart_pixel_buffer::download_viewport(GLenum format, GLenum type) {
     glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
 }
 
-void smart_pixel_buffer::upload_impl(const image_mem_info &img, smart_cuda_stream *stream) {
-    if (cuda_res_up == nullptr) {
+void *smart_pixel_buffer::up_mapped_ptr(smart_cuda_stream *stream, size_t *_ptr_size) {
+    if (cuda_res_up == nullptr) [[unlikely]] {
         CUDA_API_CHECK(cudaGraphicsGLRegisterBuffer(
                 &cuda_res_up, id, cudaGraphicsRegisterFlagsWriteDiscard));
     }
     assert(cuda_res_up != nullptr);
-
     void *ptr = nullptr;
     size_t ptr_size = 0;
     CUDA_API_CHECK(cudaGraphicsMapResources(1, &cuda_res_up, stream->cuda));
     CUDA_API_CHECK(cudaGraphicsResourceGetMappedPointer(&ptr, &ptr_size, cuda_res_up));
-    assert(ptr_size == img.width * img.height);
-    img_ptr = img.ptr;
+    if (_ptr_size != nullptr) {
+        *_ptr_size = ptr_size;
+    }
+    return ptr;
+}
+
+void smart_pixel_buffer::up_unmap(smart_cuda_stream *stream) {
+    CUDA_API_CHECK(cudaGraphicsUnmapResources(1, &cuda_res_up, stream->cuda));
+}
+
+void smart_pixel_buffer::upload_impl(const image_mem_info &img, smart_cuda_stream *stream) {
+    size_t ptr_size = 0;
+    auto ptr = up_mapped_ptr(stream, &ptr_size);
+    assert(ptr_size >= img.width * img.height);
     CUDA_API_CHECK(cudaMemcpy2DAsync(
-            ptr, img.width, img_ptr.get(), img.pitch, img.width, img.height,
+            ptr, img.width, img.ptr.get(), img.pitch, img.width, img.height,
             img.loc == MEM_CUDA ? cudaMemcpyDeviceToDevice : cudaMemcpyHostToDevice,
             stream->cuda));
-    CUDA_API_CHECK(cudaGraphicsUnmapResources(1, &cuda_res_up, stream->cuda));
+    up_unmap(stream);
+    extend_pointer_life(img.ptr, stream);
+}
+
+void smart_pixel_buffer::upload_impl(const std::shared_ptr<void> &data, size_t data_size,
+                                     memory_location loc, smart_cuda_stream *stream) {
+    create(data_size);
+
+    if (loc == MEM_CUDA) {
+        size_t ptr_size = 0;
+        auto ptr = up_mapped_ptr(stream, &ptr_size);
+        assert(ptr_size >= data_size);
+        CUDA_API_CHECK(cudaMemcpyAsync(ptr, data.get(), data_size,
+                                       cudaMemcpyHostToDevice, stream->cuda));
+        up_unmap(stream);
+        extend_pointer_life(data, stream);
+    } else {
+        assert(loc == MEM_HOST);
+        glBindBuffer(GL_ARRAY_BUFFER, id);
+        assert(size == data_size);
+        glBufferSubData(GL_ARRAY_BUFFER, 0, data_size, data.get());
+    }
 }
 
 void smart_pixel_buffer::download_impl(const image_mem_info &img, smart_cuda_stream *stream) {
@@ -263,7 +302,7 @@ void smart_pixel_buffer::download_impl(const image_mem_info &img, smart_cuda_str
     size_t ptr_size = 0;
     CUDA_API_CHECK(cudaGraphicsMapResources(1, &cuda_res_down, stream->cuda));
     CUDA_API_CHECK(cudaGraphicsResourceGetMappedPointer(&ptr, &ptr_size, cuda_res_down));
-    assert(ptr_size == img.width * img.height);
+    assert(ptr_size >= img.width * img.height);
     CUDA_API_CHECK(cudaMemcpy2DAsync(
             img.ptr.get(), img.pitch, ptr, img.width, img.width, img.height,
             img.loc == MEM_CUDA ? cudaMemcpyDeviceToDevice : cudaMemcpyDeviceToHost,

src/render/impl/shader/pc_rgb.frag

@@ -0,0 +1,9 @@
+#version 460
+
+in vec3 point_color;
+
+layout (location = 0) out vec4 frag_color;
+
+void main() {
+    frag_color = vec4(point_color, 1.0);
+}

src/render/impl/shader/pc_rgb.vert

@@ -0,0 +1,15 @@
+#version 460
+
+uniform mat4 transform;
+uniform float point_size;
+
+layout (location = 0) in vec3 position;
+layout (location = 1) in vec3 color;
+
+out vec3 point_color;
+
+void main() {
+    point_color = color;
+    gl_Position = transform * vec4(position, 1.0);
+    gl_PointSize = point_size;
+}

src/render/render_pc.h

@@ -0,0 +1,24 @@
+#ifndef DEPTHGUIDE_RENDER_PC_H
+#define DEPTHGUIDE_RENDER_PC_H
+
+#include "core/pc_utility.h"
+#include "render/render_mesh.h"
+
+struct pc_info_type {
+    pc_format_enum format; // point cloud format
+    GLuint vao; // vertex array
+    GLuint vbo; // vertex buffer
+    GLsizei num; // number of points
+    glm::mat4 transform;
+    glm::vec3 color; // ignored with PC_XYZ_RGB
+};
+
+struct pc_render_info {
+    pc_info_type pc = {};
+    camera_info_type camera = {};
+    GLfloat point_size = 1.0;
+};
+
+void render_pc(const pc_render_info &info);
+
+#endif //DEPTHGUIDE_RENDER_PC_H

src/render/render_tools.h

@@ -66,5 +66,31 @@ private:
     color_image_render color_render;
 };
 
+class pc_render {
+public:
+
+    struct config_type {
+        struct {
+            glm::mat4 transform = glm::mat4(1.0f);
+            glm::vec3 color;
+            float point_size = 1.0;
+        } pc = {};
+        struct {
+            glm::mat4 transform = glm::mat4(1.0f);
+            float fov = 60.0f;
+            float near = 1.0f, far = 1000.0f; // clip range
+        } camera = {};
+        smart_cuda_stream *stream = nullptr;
+    };
+
+    void render(obj_name_type name, const config_type &conf);
+
+private:
+    smart_opengl_buffer vbo;
+
+    GLuint vao = 0;
+    GLuint last_vbo = 0;
+    pc_format_enum last_fmt;
+};
 
 #endif //DEPTHGUIDE_RENDER_TOOLS_H

src/render/render_utility.h

@@ -3,6 +3,7 @@
 
 #include "core/cuda_helper.hpp"
 #include "core/image_utility.hpp"
+#include "core/pc_utility.h"
 
 #include <glad/gl.h>
 #include <glm/glm.hpp>
@@ -76,7 +77,6 @@ public:
     // used for CUDA inter-op
     cudaGraphicsResource_t cuda_res_up = nullptr; // for upload
     cudaGraphicsResource_t cuda_res_down = nullptr; // for download
-    std::shared_ptr<void> img_ptr;
 
     ~smart_pixel_buffer();
 
@@ -85,9 +85,14 @@ public:
     // download from current viewport
     void download_viewport(GLenum format, GLenum type);
 
+    void upload(const pc_ptr &pc, smart_cuda_stream *stream) {
+        auto pc_mem = pc->memory(MEM_HOST, stream);
+        upload_impl(pc_mem.ptr, pc->size_in_bytes(), MEM_HOST, stream);
+    }
+
     template<typename T>
     void upload(const image_info_type<T> &img, smart_cuda_stream *stream) {
-        create(sizeof(T) * img.size.area());
+        create(sizeof(T) * img.size.area()); // TODO: create inside upload_impl
         upload_impl(img.mem_info(), stream);
     }
 
@@ -108,14 +113,25 @@ public:
 
 private:
 
+    size_t capacity = 0;
+
     void deallocate();
 
+    void *up_mapped_ptr(smart_cuda_stream *stream, size_t *ptr_size = nullptr);
+
+    void up_unmap(smart_cuda_stream *stream);
+
     void upload_impl(const image_mem_info &img, smart_cuda_stream *stream);
 
+    void upload_impl(const std::shared_ptr<void> &data, size_t size,
+                     memory_location loc, smart_cuda_stream *stream);
+
     void download_impl(const image_mem_info &img, smart_cuda_stream *stream);
 
 };
 
+using smart_opengl_buffer = smart_pixel_buffer;
+
 // only upload
 class smart_texture : private boost::noncopyable {
 public: