From 6e59f48d1c83201203a68447fb39ac016877146a Mon Sep 17 00:00:00 2001 From: Lia Stratopoulos <167905060+lia-viam@users.noreply.github.com> Date: Thu, 24 Jul 2025 13:54:32 -0400 Subject: [PATCH 1/3] rewrite raw camera image and rgb to pcd --- src/module/orbbec.cpp | 91 ++++++++++++++++++++----------------------- 1 file changed, 42 insertions(+), 49 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 2ad89264..c1daed76 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -27,6 +27,8 @@ #include #include +#include + #include #include #include @@ -76,17 +78,23 @@ struct ViamOBDevice { std::shared_ptr config; }; -struct raw_camera_image { - using deleter_type = void (*)(unsigned char*); - using uniq = std::unique_ptr; +class raw_camera_image { + public: + explicit raw_camera_image(std::size_t sz) : bytes(new unsigned char[sz]), size(sz) {} - static constexpr deleter_type free_deleter = [](unsigned char* ptr) { free(ptr); }; + boost::span span() const noexcept { + return boost::span{bytes.get(), size}; + } - static constexpr deleter_type array_delete_deleter = [](unsigned char* ptr) { delete[] ptr; }; + boost::span span() noexcept { + return boost::span{bytes.get(), size}; + } - uniq bytes; - size_t size; + private: + std::unique_ptr bytes; + std::size_t size; }; + // STRUCTS END // GLOBALS BEGIN @@ -141,24 +149,24 @@ uint64_t timeSinceFrameUs(uint64_t nowUs, uint64_t imageTimeUs) { } std::vector RGBPointsToPCD(std::shared_ptr frame, float scale) { - int numPoints = frame->dataSize() / sizeof(OBColorPoint); + assert(frame->is()); + assert(frame->getFormat() == OBFormat::OB_FORMAT_RGB_POINT); + + auto pointSpan = boost::span{reinterpret_cast(frame->data()), frame->dataSize() / sizeof(OBColorPoint)}; - OBColorPoint* points = (OBColorPoint*)frame->data(); std::vector pcdPoints; - for (int i = 0; i < numPoints; i++) { - OBColorPoint& p = points[i]; - unsigned int r = (unsigned int)p.r; - unsigned int g = (unsigned int)p.g; - unsigned int b = (unsigned int)p.b; + std::transform(pointSpan.begin(), pointSpan.end(), std::back_inserter(pcdPoints), [scale](const OBColorPoint& p) { + auto r = static_cast(p.r); + auto g = static_cast(p.g); + auto b = static_cast(p.b); + unsigned int rgb = (r << 16) | (g << 8) | b; - PointXYZRGB pt; - pt.x = (p.x * scale); - pt.y = (p.y * scale); - pt.z = (p.z * scale); - pt.rgb = rgb; - pcdPoints.push_back(pt); - } + return PointXYZRGB{p.x * scale, // + p.y * scale, // + p.z * scale, // + rgb}; + }); std::stringstream header; header << "VERSION .7\n" @@ -174,32 +182,15 @@ std::vector RGBPointsToPCD(std::shared_ptr frame, floa std::string headerStr = header.str(); std::vector pcdBytes; pcdBytes.insert(pcdBytes.end(), headerStr.begin(), headerStr.end()); - for (auto& p : pcdPoints) { - unsigned char* x = (unsigned char*)&p.x; - unsigned char* y = (unsigned char*)&p.y; - unsigned char* z = (unsigned char*)&p.z; - unsigned char* rgb = (unsigned char*)&p.rgb; - - pcdBytes.push_back(x[0]); - pcdBytes.push_back(x[1]); - pcdBytes.push_back(x[2]); - pcdBytes.push_back(x[3]); - pcdBytes.push_back(y[0]); - pcdBytes.push_back(y[1]); - pcdBytes.push_back(y[2]); - pcdBytes.push_back(y[3]); + // Assert that PointXYZRGB is a POD type, and that it has no padding, thus can be copied as bytes. + // Since vector is contiguous, we can just copy the whole thing + static_assert(std::is_pod_v); + static_assert(sizeof(PointXYZRGB) == (3 * sizeof(float)) + sizeof(unsigned int), "PointXYZRGB has unexpected padding"); - pcdBytes.push_back(z[0]); - pcdBytes.push_back(z[1]); - pcdBytes.push_back(z[2]); - pcdBytes.push_back(z[3]); - - pcdBytes.push_back(rgb[0]); - pcdBytes.push_back(rgb[1]); - pcdBytes.push_back(rgb[2]); - pcdBytes.push_back(rgb[3]); - } + auto byteSpan = boost::as_bytes(boost::span{pcdPoints}); + pcdBytes.insert( + pcdBytes.end(), reinterpret_cast(byteSpan.begin()), reinterpret_cast(byteSpan.end())); return pcdBytes; } @@ -429,10 +420,12 @@ raw_camera_image encodeDepthRAW(const unsigned char* data, const uint64_t width, std::vector encodedData = viam::sdk::Camera::encode_depth_map(m); - unsigned char* rawBuf = new unsigned char[encodedData.size()]; - std::memcpy(rawBuf, encodedData.data(), encodedData.size()); + raw_camera_image rawBuf(encodedData.size()); + auto bufSpan = rawBuf.span(); + + std::copy(encodedData.begin(), encodedData.end(), bufSpan.begin()); - return raw_camera_image{raw_camera_image::uniq(rawBuf, raw_camera_image::array_delete_deleter), encodedData.size()}; + return rawBuf; } // HELPERS END @@ -682,7 +675,7 @@ vsdk::Camera::image_collection Orbbec::get_images() { vsdk::Camera::raw_image depth_image; depth_image.source_name = kDepthSourceName; depth_image.mime_type = kDepthMimeTypeViamDep; - depth_image.bytes.assign(rci.bytes.get(), rci.bytes.get() + rci.size); + depth_image.bytes.assign(rci.span().begin(), rci.span().end()); vsdk::Camera::image_collection response; response.images.emplace_back(std::move(color_image)); From c7e807c1926ac1c62ab85be12e97d7ce697911bf Mon Sep 17 00:00:00 2001 From: Lia Stratopoulos <167905060+lia-viam@users.noreply.github.com> Date: Thu, 24 Jul 2025 15:28:32 -0400 Subject: [PATCH 2/3] condense helper invocation --- src/module/orbbec.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index c1daed76..04f122b8 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -412,18 +412,17 @@ void listDevices(const ob::Context& ctx) { raw_camera_image encodeDepthRAW(const unsigned char* data, const uint64_t width, const uint64_t height, const bool littleEndian) { viam::sdk::Camera::depth_map m = xt::xarray::from_shape({height, width}); - std::copy(reinterpret_cast(data), reinterpret_cast(data) + height * width, m.begin()); - for (size_t i = 0; i < m.size(); i++) { - m[i] = m[i] * mmToMeterMultiple; - } + std::transform( + reinterpret_cast(data), reinterpret_cast(data) + (height * width), m.begin(), [](uint16_t mi) { + return mi * mmToMeterMultiple; + }); std::vector encodedData = viam::sdk::Camera::encode_depth_map(m); raw_camera_image rawBuf(encodedData.size()); - auto bufSpan = rawBuf.span(); - std::copy(encodedData.begin(), encodedData.end(), bufSpan.begin()); + std::copy(encodedData.begin(), encodedData.end(), rawBuf.span().begin()); return rawBuf; } From c0c7fe6941d6fe298b8a16792744a1acad6f5f43 Mon Sep 17 00:00:00 2001 From: Lia Stratopoulos <167905060+lia-viam@users.noreply.github.com> Date: Fri, 25 Jul 2025 10:30:01 -0400 Subject: [PATCH 3/3] use span --- src/module/orbbec.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/module/orbbec.cpp b/src/module/orbbec.cpp index 04f122b8..bef2201d 100644 --- a/src/module/orbbec.cpp +++ b/src/module/orbbec.cpp @@ -413,10 +413,9 @@ void listDevices(const ob::Context& ctx) { raw_camera_image encodeDepthRAW(const unsigned char* data, const uint64_t width, const uint64_t height, const bool littleEndian) { viam::sdk::Camera::depth_map m = xt::xarray::from_shape({height, width}); - std::transform( - reinterpret_cast(data), reinterpret_cast(data) + (height * width), m.begin(), [](uint16_t mi) { - return mi * mmToMeterMultiple; - }); + auto depthPixels = boost::span{reinterpret_cast(data), height * width}; + + std::transform(depthPixels.begin(), depthPixels.end(), m.begin(), [](uint16_t mi) { return mi * mmToMeterMultiple; }); std::vector encodedData = viam::sdk::Camera::encode_depth_map(m);