From 3cd35fc48e03df0807cd612e964eb131bb4035cb Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 31 Oct 2022 16:16:21 +0900 Subject: [PATCH 01/14] add zdepth package --- 3rdparty/zdepth/CMakeLists.txt | 17 ++++++++++++++ 3rdparty/zdepth/fix_cmakelists.patch | 34 ++++++++++++++++++++++++++++ 3rdparty/zdepth/package.xml | 16 +++++++++++++ 3 files changed, 67 insertions(+) create mode 100644 3rdparty/zdepth/CMakeLists.txt create mode 100644 3rdparty/zdepth/fix_cmakelists.patch create mode 100644 3rdparty/zdepth/package.xml diff --git a/3rdparty/zdepth/CMakeLists.txt b/3rdparty/zdepth/CMakeLists.txt new file mode 100644 index 000000000..5bd879795 --- /dev/null +++ b/3rdparty/zdepth/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zdepth) + +include(ExternalProject) + +ExternalProject_Add( + zdepth + GIT_REPOSITORY https://github.com/catid/Zdepth.git + GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 + GIT_SHALLOW TRUE + PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 + CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ + USE_SOURCE_PERMISSIONS) diff --git a/3rdparty/zdepth/fix_cmakelists.patch b/3rdparty/zdepth/fix_cmakelists.patch new file mode 100644 index 000000000..7ea697610 --- /dev/null +++ b/3rdparty/zdepth/fix_cmakelists.patch @@ -0,0 +1,34 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index b22b2e7..a589f4d 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -80,10 +80,13 @@ target_link_libraries(zdepth_test PRIVATE zdepth) + ################################################################################ + # Install + +-install(FILES LICENSE DESTINATION ${CMAKE_INSTALL_PREFIX}) +-install(FILES README.md DESTINATION ${CMAKE_INSTALL_PREFIX}) +-install(DIRECTORY cmake include DESTINATION ${CMAKE_INSTALL_PREFIX}) +-install(TARGETS zdepth EXPORT zdepth LIBRARY DESTINATION lib) ++# install(FILES LICENSE DESTINATION ${CMAKE_INSTALL_PREFIX}) ++# install(FILES README.md DESTINATION ${CMAKE_INSTALL_PREFIX}) ++install(FILES include/zdepth.hpp DESTINATION ${CMAKE_INSTALL_PREFIX}/include/zdepth) ++install(FILES tests/test_vectors.inl DESTINATION ${CMAKE_INSTALL_PREFIX}/include/zdepth/tests) ++install(TARGETS zdepth EXPORT zdepth ++ ARCHIVE DESTINATION lib ++ LIBRARY DESTINATION lib) + + ################################################################################ + # Generate zdepthConfig.cmake and zdepthConfigVersion.cmake for cmake projects +diff --git a/zstd/CMakeLists.txt b/zstd/CMakeLists.txt +index 5869d4d..c0aa503 100644 +--- a/zstd/CMakeLists.txt ++++ b/zstd/CMakeLists.txt +@@ -25,4 +25,6 @@ target_include_directories(zstd PUBLIC + "$" + ) + +-install(TARGETS zstd EXPORT zdepth LIBRARY DESTINATION lib) ++install(TARGETS zstd EXPORT zdepth ++ ARCHIVE DESTINATION lib ++ LIBRARY DESTINATION lib) diff --git a/3rdparty/zdepth/package.xml b/3rdparty/zdepth/package.xml new file mode 100644 index 000000000..7f9107564 --- /dev/null +++ b/3rdparty/zdepth/package.xml @@ -0,0 +1,16 @@ + + + zdepth + 0.0.0 + The zdepth package + + Shingo Kitagawa + + BSD + + catkin + + + + + From 923f87d23ae0122e8c64fbcc8cd2a3be6bdce8bd Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:15:14 +0900 Subject: [PATCH 02/14] build zdepth as shared library --- 3rdparty/zdepth/fix_cmakelists.patch | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/3rdparty/zdepth/fix_cmakelists.patch b/3rdparty/zdepth/fix_cmakelists.patch index 7ea697610..cc5191852 100644 --- a/3rdparty/zdepth/fix_cmakelists.patch +++ b/3rdparty/zdepth/fix_cmakelists.patch @@ -1,7 +1,16 @@ diff --git a/CMakeLists.txt b/CMakeLists.txt -index b22b2e7..a589f4d 100644 +index b22b2e7..a4251af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt +@@ -61,7 +61,7 @@ include_directories(include) + + # zdepth library + +-add_library(zdepth STATIC ${SOURCE_FILES}) ++add_library(zdepth SHARED ${SOURCE_FILES}) + add_library(zdepth::zdepth ALIAS zdepth) + + target_link_libraries(zdepth PUBLIC zstd) @@ -80,10 +80,13 @@ target_link_libraries(zdepth_test PRIVATE zdepth) ################################################################################ # Install @@ -21,10 +30,17 @@ index b22b2e7..a589f4d 100644 ################################################################################ # Generate zdepthConfig.cmake and zdepthConfigVersion.cmake for cmake projects diff --git a/zstd/CMakeLists.txt b/zstd/CMakeLists.txt -index 5869d4d..c0aa503 100644 +index 5869d4d..f9339a3 100644 --- a/zstd/CMakeLists.txt +++ b/zstd/CMakeLists.txt -@@ -25,4 +25,6 @@ target_include_directories(zstd PUBLIC +@@ -19,10 +19,12 @@ include_directories(include src) + ################################################################################ + # Targets + +-add_library(zstd ${ZSTD_SOURCE_FILES}) ++add_library(zstd SHARED ${ZSTD_SOURCE_FILES}) + target_include_directories(zstd PUBLIC + "$" "$" ) From 61c14b7925f633795469070362e2f6132d3760b5 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:29:15 +0900 Subject: [PATCH 03/14] update package information --- 3rdparty/zdepth/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/3rdparty/zdepth/package.xml b/3rdparty/zdepth/package.xml index 7f9107564..fe4d3cde8 100644 --- a/3rdparty/zdepth/package.xml +++ b/3rdparty/zdepth/package.xml @@ -4,7 +4,7 @@ 0.0.0 The zdepth package - Shingo Kitagawa + Shingo Kitagawa BSD From c917bb58ca37ea3ce8a7335ec1b5234df0dedd7d Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:29:22 +0900 Subject: [PATCH 04/14] add publisher plugin and zdepth_image_transport --- .../zdepth_image_transport/CMakeLists.txt | 71 ++++ .../zdepth_image_transport/zdepth_publisher.h | 22 ++ .../msg/ZDepthImage.msg | 15 + 3rdparty/zdepth_image_transport/package.xml | 31 ++ .../zdepth_image_transport/src/manifest.cpp | 6 + .../src/zdepth_publisher.cpp | 44 +++ .../src/zdepth_test.cpp | 368 ++++++++++++++++++ .../zdepth_image_transport/zdepth_plugins.xml | 15 + 8 files changed, 572 insertions(+) create mode 100644 3rdparty/zdepth_image_transport/CMakeLists.txt create mode 100644 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h create mode 100644 3rdparty/zdepth_image_transport/msg/ZDepthImage.msg create mode 100644 3rdparty/zdepth_image_transport/package.xml create mode 100644 3rdparty/zdepth_image_transport/src/manifest.cpp create mode 100644 3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp create mode 100644 3rdparty/zdepth_image_transport/src/zdepth_test.cpp create mode 100644 3rdparty/zdepth_image_transport/zdepth_plugins.xml diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt new file mode 100644 index 000000000..90ed3ea85 --- /dev/null +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zdepth_image_transport) + +find_package(catkin REQUIRED + COMPONENTS + image_transport + message_generation + pluginlib + roscpp + sensor_msgs + std_msgs +) + +find_library(zdepth_LIB zdepth PATH_SUFFIXES zdepth) +find_library(zstd_LIB zstd PATH_SUFFIXES zdepth) +set(zdepth_LIBRARIES ${zdepth_LIB} ${zstd_LIB}) +find_file(zdepth_INCLUDE zdepth.hpp PATH_SUFFIXES zdepth) +get_filename_component(zdepth_PATH ${zdepth_INCLUDE} DIRECTORY) +get_filename_component(zdepth_INCLUDE_DIRS ${zdepth_PATH} DIRECTORY) + +add_message_files( + DIRECTORY msg + FILES + ZDepthImage.msg +) + +generate_messages(DEPENDENCIES std_msgs) + + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + message_runtime + image_transport + pluginlib + roscpp + sensor_msgs + std_msgs + DEPENDS + zdepth +) + +include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) + +add_executable(zdepth_test src/zdepth_test.cpp) +target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + +# add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) +add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + +install(TARGETS zdepth_test + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES zdepth_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h new file mode 100644 index 000000000..4c86c5215 --- /dev/null +++ b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h @@ -0,0 +1,22 @@ +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/ZDepthImage.h" + + +namespace zdepth_image_transport +{ + class ZDepthPublisher : public image_transport::SimplePublisherPlugin + { + public: + virtual ~ZDepthPublisher() {} + virtual std::string getTransportName() const + { + return "zdepth"; + } + + protected: + virtual void publish(const sensor_msgs::Image& img_msg, + const PublishFn& publish_fn) const; + }; + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg b/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg new file mode 100644 index 000000000..8cb12028e --- /dev/null +++ b/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg @@ -0,0 +1,15 @@ +# This message contains a compressed image + +Header header # Header timestamp should be acquisition time of image + # Header frame_id should be optical frame of camera + # origin of frame should be optical center of camera + # +x should point to the right in the image + # +y should point down in the image + # +z should point into to plane of the image + +string format # Specifies the format of the data + # Acceptable values: + # jpeg, png +uint8[] data # Compressed image buffer +int32 width +int32 height diff --git a/3rdparty/zdepth_image_transport/package.xml b/3rdparty/zdepth_image_transport/package.xml new file mode 100644 index 000000000..7a2d2cbd6 --- /dev/null +++ b/3rdparty/zdepth_image_transport/package.xml @@ -0,0 +1,31 @@ + + + zdepth_image_transport + 0.0.0 + The zdepth_image_transport package + + Shingo Kitagawa + + BSD + + + catkin + image_transport + message_generation + pluginlib + roscpp + sensor_msgs + std_msgs + zdepth + + image_transport + message_runtime + pluginlib + roscpp + sensor_msgs + std_msgs + zdepth + + + + diff --git a/3rdparty/zdepth_image_transport/src/manifest.cpp b/3rdparty/zdepth_image_transport/src/manifest.cpp new file mode 100644 index 000000000..fdb5a02ef --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/manifest.cpp @@ -0,0 +1,6 @@ +#include +#include +// #include + +PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthPublisher, image_transport::PublisherPlugin) +// PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthSubscriber, image_transport::SubscriberPlugin) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp new file mode 100644 index 000000000..3502ccc79 --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp @@ -0,0 +1,44 @@ +// #include + +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/zdepth_publisher.h" + +#include +#include "zdepth_image_transport/ZDepthImage.h" + +namespace zdepth_image_transport +{ + + void ZDepthPublisher::publish(const sensor_msgs::Image& img_msg, + const PublishFn& publish_fn) const + { + if (img_msg.encoding != "16UC1") + { + ROS_ERROR("Unsupported encoding: %s", img_msg.encoding.c_str()); + return; + } + + zdepth::DepthCompressor compressor; + std::vector compressed; + int width = img_msg.width; + int height = img_msg.height; + + const uint16_t* frame = (uint16_t*)img_msg.data.data(); + zdepth::DepthResult result = compressor.Compress(width, height, frame, compressed, true); + if (result != zdepth::DepthResult::Success) { + ROS_ERROR("Failed to compress"); + return; + } + + // publish + zdepth_image_transport::ZDepthImage::Ptr compressed_img_msg(new zdepth_image_transport::ZDepthImage()); + compressed_img_msg->header = img_msg.header; + compressed_img_msg->data = compressed; + compressed_img_msg->format = "16UC1; zdepth"; + compressed_img_msg->width = width; + compressed_img_msg->height = height; + publish_fn(*compressed_img_msg); + } + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/src/zdepth_test.cpp b/3rdparty/zdepth_image_transport/src/zdepth_test.cpp new file mode 100644 index 000000000..5d2babf7f --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/zdepth_test.cpp @@ -0,0 +1,368 @@ +// Copyright 2019 (c) Christopher A. Taylor. All rights reserved. + +#include "zdepth/zdepth.hpp" +using namespace zdepth; + + +//------------------------------------------------------------------------------ +// Timing + +#ifdef _WIN32 + #ifndef NOMINMAX + #define NOMINMAX + #endif + #include +#elif __MACH__ + #include + #include + #include + #include + + extern mach_port_t clock_port; +#else + #include + #include + #include // flock +#endif + +#ifdef _WIN32 +// Precomputed frequency inverse +static double PerfFrequencyInverseUsec = 0.; +static double PerfFrequencyInverseMsec = 0.; + +static void InitPerfFrequencyInverse() +{ + LARGE_INTEGER freq = {}; + if (!::QueryPerformanceFrequency(&freq) || freq.QuadPart == 0) { + return; + } + const double invFreq = 1. / (double)freq.QuadPart; + PerfFrequencyInverseUsec = 1000000. * invFreq; + PerfFrequencyInverseMsec = 1000. * invFreq; +} +#elif __MACH__ +static bool m_clock_serv_init = false; +static clock_serv_t m_clock_serv = 0; + +static void InitClockServ() +{ + m_clock_serv_init = true; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &m_clock_serv); +} +#endif // _WIN32 + +uint64_t GetTimeUsec() +{ +#ifdef _WIN32 + LARGE_INTEGER timeStamp = {}; + if (!::QueryPerformanceCounter(&timeStamp)) { + return 0; + } + if (PerfFrequencyInverseUsec == 0.) { + InitPerfFrequencyInverse(); + } + return (uint64_t)(PerfFrequencyInverseUsec * timeStamp.QuadPart); +#elif __MACH__ + if (!m_clock_serv_init) { + InitClockServ(); + } + + mach_timespec_t tv; + clock_get_time(m_clock_serv, &tv); + + return 1000000 * tv.tv_sec + tv.tv_nsec / 1000; +#else + // This seems to be the best clock to used based on: + // http://btorpey.github.io/blog/2014/02/18/clock-sources-in-linux/ + // The CLOCK_MONOTONIC_RAW seems to take a long time to query, + // and so it only seems useful for timing code that runs a small number of times. + // The CLOCK_MONOTONIC is affected by NTP at 500ppm but doesn't make sudden jumps. + // Applications should already be robust to clock skew so this is acceptable. + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_nsec / 1000) + static_cast(ts.tv_sec) * 1000000; +#endif +} + + +//------------------------------------------------------------------------------ +// RVL + +// RVL library for performance baseline + +// Paper: https://www.microsoft.com/en-us/research/publication/fast-lossless-depth-image-compression/ +// Video presentation: https://www.youtube.com/watch?v=WYU2upBs2hA + +// RVL author suggests that H.264 is a bad idea to use. +// But it seems like some masking can be used to avoid messing up the edges... + +// Effective Compression of Range Data Streams for Remote Robot Operations using H.264 +// http://www2.informatik.uni-freiburg.de/~stachnis/pdf/nenci14iros.pdf + +// Adapting Standard Video Codecs for Depth Streaming +// http://reality.cs.ucl.ac.uk/projects/depth-streaming/depth-streaming.pdf + +inline void EncodeVLE(int* &pBuffer, int& word, int& nibblesWritten, int value) +{ + do + { + int nibble = value & 0x7; // lower 3 bits + if (value >>= 3) { + nibble |= 0x8; // more to come + } + word <<= 4; + word |= nibble; + if (++nibblesWritten == 8) // output word + { + *pBuffer++ = word; + nibblesWritten = 0; + word = 0; + } + } while (value); +} + +inline int DecodeVLE(int* &pBuffer, int& word, int& nibblesWritten) +{ + unsigned int nibble; + int value = 0, bits = 29; + do + { + if (!nibblesWritten) + { + word = *pBuffer++; // load word + nibblesWritten = 8; + } + nibble = word & 0xf0000000; + value |= (nibble << 1) >> bits; + word <<= 4; + nibblesWritten--; + bits -= 3; + } while (nibble & 0x80000000); + return value; +} + +int CompressRVL(short* input, char* output, int numPixels) +{ + int word{ 0 }; + int nibblesWritten; + int *pBuffer; + int *buffer = pBuffer = (int*)output; + nibblesWritten = 0; + short *end = input + numPixels; + short previous = 0; + while (input != end) + { + int zeros = 0, nonzeros = 0; + for (; (input != end) && !*input; input++, zeros++); + EncodeVLE(pBuffer, word, nibblesWritten, zeros); // number of zeros + for (short* p = input; (p != end) && *p++; nonzeros++); + EncodeVLE(pBuffer, word, nibblesWritten, nonzeros); // number of nonzeros + for (int i = 0; i < nonzeros; i++) + { + short current = *input++; + int delta = current - previous; + int positive = (delta << 1) ^ (delta >> 31); + EncodeVLE(pBuffer, word, nibblesWritten, positive); // nonzero value + previous = current; + } + } + if (nibblesWritten) // last few values + { + *pBuffer++ = word << 4 * (8 - nibblesWritten); + } + return int((char*)pBuffer - (char*)buffer); // num bytes +} + +void DecompressRVL(char* input, short* output, int numPixels) +{ + int word, nibblesWritten; + int *pBuffer = (int*)input; + nibblesWritten = 0; + short current, previous = 0; + int numPixelsToDecode = numPixels; + while (numPixelsToDecode) + { + int zeros = DecodeVLE(pBuffer, word, nibblesWritten); // number of zeros + numPixelsToDecode -= zeros; + for (; zeros; zeros--) { + *output++ = 0; + } + int nonzeros = DecodeVLE(pBuffer, word, nibblesWritten); // number of nonzeros + numPixelsToDecode -= nonzeros; + for (; nonzeros; nonzeros--) + { + int positive = DecodeVLE(pBuffer, word, nibblesWritten); // nonzero value + int delta = (positive >> 1) ^ -(positive & 1); + current = previous + static_cast(delta); + *output++ = current; + previous = current; + } + } +} + + +//------------------------------------------------------------------------------ +// Test Vectors + +#include "zdepth/tests/test_vectors.inl" + + +//------------------------------------------------------------------------------ +// Test Application + +#include +using namespace std; + +static zdepth::DepthCompressor compressor, decompressor; + +bool TestFrame(const uint16_t* frame, bool keyframe) +{ + std::vector compressed; + + static_assert(Width % kBlockSize == 0, "Width is not a multiple of block size."); + static_assert(Height % kBlockSize == 0, "Height is not a multiple of block size."); + + const uint64_t t0 = GetTimeUsec(); + + compressor.Compress(Width, Height, frame, compressed, keyframe); + + const uint64_t t1 = GetTimeUsec(); + + int width, height; + std::vector depth; + zdepth::DepthResult result = decompressor.Decompress(compressed, width, height, depth); + + const uint64_t t2 = GetTimeUsec(); + + if (result != zdepth::DepthResult::Success) { + cout << "Failed: decompressor.Decompress returned " << zdepth::DepthResultString(result) << endl; + return false; + } + if (width != Width || + height != Height) + { + cout << "Decompression failed: Resolution" << endl; + return false; + } + + for (int i = 0; i < depth.size(); ++i) { + if (AzureKinectQuantizeDepth(depth[i]) != AzureKinectQuantizeDepth(frame[i])) { + cout << "Decompression failed: Contents did not match at offset = " << i << endl; + return false; + } + } + + const unsigned original_bytes = Width * Height * 2; + cout << endl; + cout << "Zdepth Compression: " << original_bytes << " bytes -> " << compressed.size() << + " bytes (ratio = " << original_bytes / (float)compressed.size() << ":1) (" + << (compressed.size() * 30 * 8) / 1000000.f << " Mbps @ 30 FPS)" << endl; + cout << "Zdepth Speed: Compressed in " << (t1 - t0) / 1000.f << " msec. Decompressed in " << (t2 - t1) / 1000.f << " msec" << endl; + + const int n = Width * Height; + std::vector quantized(n); + compressed.resize(n * 3); + + const uint64_t t3 = GetTimeUsec(); + QuantizeDepthImage(Width, Height, frame, quantized); + const int compressed_bytes = CompressRVL((short*)quantized.data(), (char*)compressed.data(), n); + compressed.resize(compressed_bytes); + const uint64_t t4 = GetTimeUsec(); + + std::vector recompressed; + std::vector decompressed; + + const uint64_t t5 = GetTimeUsec(); + ZstdCompress(compressed, recompressed); + const uint64_t t6 = GetTimeUsec(); + ZstdDecompress(recompressed.data(), static_cast(recompressed.size()), static_cast(compressed.size()), decompressed); + const uint64_t t7 = GetTimeUsec(); + quantized.resize(n * 2); + DecompressRVL((char*)decompressed.data(), (short*)quantized.data(), n); + DequantizeDepthImage(Width, Height, quantized.data(), depth); + const uint64_t t8 = GetTimeUsec(); + + for (int i = 0; i < depth.size(); ++i) { + if (AzureKinectQuantizeDepth(depth[i]) != AzureKinectQuantizeDepth(frame[i])) { + cout << "Decompression failed: Contents did not match at offset = " << i << endl; + return false; + } + } + + cout << endl; + cout << "Quantization+RVL Compression: " << original_bytes << " bytes -> " << compressed.size() << + " bytes (ratio = " << original_bytes / (float)compressed.size() << ":1) (" + << (compressed.size() * 30 * 8) / 1000000.f << " Mbps @ 30 FPS)" << endl; + cout << "Quantization+RVL Speed: Compressed in " << (t4 - t3) / 1000.f << " msec. Decompressed in " << (t8 - t7) / 1000.f << " msec" << endl; + + cout << endl; + cout << "Quantization+RVL+Zstd Compression: " << original_bytes << " bytes -> " << recompressed.size() << + " bytes (ratio = " << original_bytes / (float)recompressed.size() << ":1) (" + << (recompressed.size() * 30 * 8) / 1000000.f << " Mbps @ 30 FPS)" << endl; + cout << "Quantization+RVL+Zstd Speed: Compressed in " << (t6 - t5 + t4 - t3) / 1000.f << " msec. Decompressed in " << (t8 - t6) / 1000.f << " msec" << endl; + + return true; +} + +bool TestPattern(const uint16_t* frame0, const uint16_t* frame1) +{ + cout << endl; + cout << "===================================================================" << endl; + cout << "+ Test: Frame 0 Keyframe=true compression" << endl; + cout << "===================================================================" << endl; + + if (!TestFrame(frame0, true)) { + cout << "Failure: frame0 failed"; + return false; + } + + cout << endl; + cout << "===================================================================" << endl; + cout << "+ Test: Frame 1 Keyframe=false compression" << endl; + cout << "===================================================================" << endl; + + if (!TestFrame(frame1, false)) { + cout << "Failure: frame1 failed"; + return false; + } + return true; +} + +int main(int argc, char* argv[]) +{ + (void)(argc); (void)(argv); + + cout << endl; + cout << "-------------------------------------------------------------------" << endl; + cout << "Test vector: Room" << endl; + cout << "-------------------------------------------------------------------" << endl; + + if (!TestPattern(TestVector0_Room0, TestVector0_Room1)) { + cout << "Test failure: Room test vector" << endl; + return -1; + } + + cout << endl; + cout << "-------------------------------------------------------------------" << endl; + cout << "Test vector: Ceiling" << endl; + cout << "-------------------------------------------------------------------" << endl; + + if (!TestPattern(TestVector1_Ceiling0, TestVector1_Ceiling1)) { + cout << "Test failure: Ceiling test vector" << endl; + return -2; + } + + cout << endl; + cout << "-------------------------------------------------------------------" << endl; + cout << "Test vector: Person" << endl; + cout << "-------------------------------------------------------------------" << endl; + + if (!TestPattern(TestVector2_Person0, TestVector2_Person1)) { + cout << "Test failure: Person test vector" << endl; + return -3; + } + cout << endl; + + cout << "Test success" << endl; + return 0; +} diff --git a/3rdparty/zdepth_image_transport/zdepth_plugins.xml b/3rdparty/zdepth_image_transport/zdepth_plugins.xml new file mode 100644 index 000000000..400bedaf8 --- /dev/null +++ b/3rdparty/zdepth_image_transport/zdepth_plugins.xml @@ -0,0 +1,15 @@ + + + + This plugin publishes a zdepth version of the image. + + + + + From d909a1f1e93032b2238a307771a49653ed5e2ce7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:48:00 +0900 Subject: [PATCH 05/14] add zdepth subsriber plugin --- .../zdepth_image_transport/CMakeLists.txt | 3 +- .../zdepth_subscriber.h | 22 ++++++++ .../zdepth_image_transport/src/manifest.cpp | 4 +- .../src/zdepth_subscriber.cpp | 54 +++++++++++++++++++ .../zdepth_image_transport/zdepth_plugins.xml | 2 - 5 files changed, 79 insertions(+), 6 deletions(-) create mode 100644 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h create mode 100644 3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt index 90ed3ea85..6441083f7 100644 --- a/3rdparty/zdepth_image_transport/CMakeLists.txt +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -46,8 +46,7 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) add_executable(zdepth_test src/zdepth_test.cpp) target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) -# add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) -add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp) +add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) install(TARGETS zdepth_test diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h new file mode 100644 index 000000000..00f348c87 --- /dev/null +++ b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h @@ -0,0 +1,22 @@ +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/ZDepthImage.h" + + +namespace zdepth_image_transport +{ + class ZDepthSubscriber : public image_transport::SimpleSubscriberPlugin + { + public: + virtual ~ZDepthSubscriber() {} + virtual std::string getTransportName() const + { + return "zdepth"; + } + + protected: + virtual void internalCallback(const zdepth_image_transport::ZDepthImageConstPtr& zdepth_msg, + const Callback& user_cb); + }; + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/src/manifest.cpp b/3rdparty/zdepth_image_transport/src/manifest.cpp index fdb5a02ef..5075e5a2e 100644 --- a/3rdparty/zdepth_image_transport/src/manifest.cpp +++ b/3rdparty/zdepth_image_transport/src/manifest.cpp @@ -1,6 +1,6 @@ #include #include -// #include +#include PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthPublisher, image_transport::PublisherPlugin) -// PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthSubscriber, image_transport::SubscriberPlugin) +PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthSubscriber, image_transport::SubscriberPlugin) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp new file mode 100644 index 000000000..8f01bd7bf --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp @@ -0,0 +1,54 @@ +// #include + +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/zdepth_subscriber.h" + +#include +#include "zdepth_image_transport/ZDepthImage.h" + +namespace zdepth_image_transport +{ + void ZDepthSubscriber::internalCallback(const zdepth_image_transport::ZDepthImageConstPtr& zdepth_msg, + const Callback& user_cb) + { + if (zdepth_msg->format != "16UC1; zdepth") + { + ROS_ERROR("Unsupported format: %s", zdepth_msg->format.c_str()); + return; + } + + zdepth::DepthCompressor decompressor; + std::vector decompressed; + int width = zdepth_msg->width; + int height = zdepth_msg->height; + + std::vector depth; + zdepth::DepthResult result = decompressor.Decompress( + zdepth_msg->data, width, height, depth + ); + + if (result != zdepth::DepthResult::Success) { + ROS_ERROR("Failed to decompress"); + return; + } + + std::vector depth_vec; + for (int i=0; i(&depth[i]); + std::vector dv(d, d+2); + depth_vec.insert(depth_vec.end(), dv.begin(), dv.end()); + } + + // callback + sensor_msgs::Image::Ptr img_msg(new sensor_msgs::Image()); + img_msg->header = zdepth_msg->header; + img_msg->data = depth_vec; + img_msg->width = width; + img_msg->height = height; + img_msg->encoding = "16UC1"; + img_msg->step = width * 2; + user_cb(img_msg); + } + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/zdepth_plugins.xml b/3rdparty/zdepth_image_transport/zdepth_plugins.xml index 400bedaf8..b9de359a9 100644 --- a/3rdparty/zdepth_image_transport/zdepth_plugins.xml +++ b/3rdparty/zdepth_image_transport/zdepth_plugins.xml @@ -5,11 +5,9 @@ - From 92ae83c8a8719fa9a415e08a0b1a71d05fbb6ac9 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 3 Nov 2022 02:05:38 +0900 Subject: [PATCH 06/14] make decompress copy faster --- .../src/zdepth_subscriber.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp index 8f01bd7bf..baed84c15 100644 --- a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp +++ b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp @@ -1,5 +1,6 @@ // #include +#include #include #include "zdepth/zdepth.hpp" #include "zdepth_image_transport/zdepth_subscriber.h" @@ -23,22 +24,26 @@ namespace zdepth_image_transport int width = zdepth_msg->width; int height = zdepth_msg->height; + auto start_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); std::vector depth; zdepth::DepthResult result = decompressor.Decompress( zdepth_msg->data, width, height, depth ); + auto decompress_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); if (result != zdepth::DepthResult::Success) { ROS_ERROR("Failed to decompress"); return; } - std::vector depth_vec; - for (int i=0; i(&depth[i]); - std::vector dv(d, d+2); - depth_vec.insert(depth_vec.end(), dv.begin(), dv.end()); - } + uint8_t* depth_vec_beg = reinterpret_cast(&depth[0]); + uint8_t* depth_vec_end = depth_vec_beg + depth.size() * 2; + std::vector depth_vec(depth_vec_beg, depth_vec_end); + auto copy_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // DEBUG + ROS_DEBUG_STREAM("decompress: " << decompress_time - start_time << "ms"); + ROS_DEBUG_STREAM("copy : " << copy_time - decompress_time << "ms"); // callback sensor_msgs::Image::Ptr img_msg(new sensor_msgs::Image()); From 050ff04bd148af4955d61a27edaadb2cbb3ea96c Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 3 Nov 2022 02:19:21 +0900 Subject: [PATCH 07/14] move zdepth_test.cpp --- 3rdparty/zdepth_image_transport/CMakeLists.txt | 2 +- 3rdparty/zdepth_image_transport/src/{ => test}/zdepth_test.cpp | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename 3rdparty/zdepth_image_transport/src/{ => test}/zdepth_test.cpp (100%) diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt index 6441083f7..1824acee8 100644 --- a/3rdparty/zdepth_image_transport/CMakeLists.txt +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -43,7 +43,7 @@ catkin_package( include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) -add_executable(zdepth_test src/zdepth_test.cpp) +add_executable(zdepth_test src/test/zdepth_test.cpp) target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_test.cpp b/3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/zdepth_test.cpp rename to 3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp From e982668294d12a0d32f923234567e574ce0e3c59 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 16 Dec 2022 20:36:20 +0900 Subject: [PATCH 08/14] fix dependencies in zdepth_image_transport --- 3rdparty/zdepth_image_transport/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt index 1824acee8..227f6cf77 100644 --- a/3rdparty/zdepth_image_transport/CMakeLists.txt +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -48,6 +48,7 @@ target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) install(TARGETS zdepth_test ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From f5095cb20ea1c15f3e60da12c940e6c42fb020b7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 19 Dec 2022 18:09:58 +0900 Subject: [PATCH 09/14] fix zdepth package --- 3rdparty/zdepth/CMakeLists.txt | 8 ++++---- 3rdparty/zdepth/package.xml | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/3rdparty/zdepth/CMakeLists.txt b/3rdparty/zdepth/CMakeLists.txt index 5bd879795..392eed536 100644 --- a/3rdparty/zdepth/CMakeLists.txt +++ b/3rdparty/zdepth/CMakeLists.txt @@ -9,9 +9,9 @@ ExternalProject_Add( GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 GIT_SHALLOW TRUE PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 - CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} + CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INSTALL_COMMAND echo "install" ) -install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ - USE_SOURCE_PERMISSIONS) + +install(CODE "execute_process(COMMAND make install WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/zdepth-prefix/src/zdepth-build)") diff --git a/3rdparty/zdepth/package.xml b/3rdparty/zdepth/package.xml index fe4d3cde8..c4b511c4d 100644 --- a/3rdparty/zdepth/package.xml +++ b/3rdparty/zdepth/package.xml @@ -12,5 +12,6 @@ + cmake From 20fb79bdb73ce808d8c181447560dc428f072874 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 00:45:10 +0900 Subject: [PATCH 10/14] comment out debug outputs --- 3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp index baed84c15..c7031cf72 100644 --- a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp +++ b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp @@ -42,8 +42,8 @@ namespace zdepth_image_transport auto copy_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); // DEBUG - ROS_DEBUG_STREAM("decompress: " << decompress_time - start_time << "ms"); - ROS_DEBUG_STREAM("copy : " << copy_time - decompress_time << "ms"); + // ROS_DEBUG_STREAM("decompress: " << decompress_time - start_time << "ms"); + // ROS_DEBUG_STREAM("copy : " << copy_time - decompress_time << "ms"); // callback sensor_msgs::Image::Ptr img_msg(new sensor_msgs::Image()); From 7ad329c4f0e72571d709f1e4888b3ba989aebb91 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 00:45:26 +0900 Subject: [PATCH 11/14] support 32fc1 --- .../src/zdepth_publisher.cpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp index 3502ccc79..32bbbb918 100644 --- a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp +++ b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp @@ -13,7 +13,7 @@ namespace zdepth_image_transport void ZDepthPublisher::publish(const sensor_msgs::Image& img_msg, const PublishFn& publish_fn) const { - if (img_msg.encoding != "16UC1") + if (img_msg.encoding != "16UC1" && img_msg.encoding != "32FC1") { ROS_ERROR("Unsupported encoding: %s", img_msg.encoding.c_str()); return; @@ -23,9 +23,26 @@ namespace zdepth_image_transport std::vector compressed; int width = img_msg.width; int height = img_msg.height; + zdepth::DepthResult result; + + if (img_msg.encoding == "16UC1") + { + const uint16_t* frame = (uint16_t*)img_msg.data.data(); + result = compressor.Compress(width, height, frame, compressed, true); + } + else + { + // convert 32fc1 -> 16uc1 + const float* frame_32fc1 = reinterpret_cast(img_msg.data.data()); + uint16_t* frame_16uc1 = new uint16_t[height*width](); + for (unsigned index = 0; index < height * width; ++index) + { + float d = frame_32fc1[index]; + frame_16uc1[index] = std::isnan(d) ? 0 : (uint16_t)(d * 1000); + } + result = compressor.Compress(width, height, frame_16uc1, compressed, true); + } - const uint16_t* frame = (uint16_t*)img_msg.data.data(); - zdepth::DepthResult result = compressor.Compress(width, height, frame, compressed, true); if (result != zdepth::DepthResult::Success) { ROS_ERROR("Failed to compress"); return; From 7003d1ea5a0f5a5d5bf5d527cf9d7cd335d833b7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 00:46:04 +0900 Subject: [PATCH 12/14] add zdepth,zdepth_image_transport in jsk_3rdparty depends --- jsk_3rdparty/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_3rdparty/package.xml b/jsk_3rdparty/package.xml index 0e546d244..ccdb96994 100644 --- a/jsk_3rdparty/package.xml +++ b/jsk_3rdparty/package.xml @@ -36,6 +36,7 @@ sesame_ros switchbot_ros webrtcvad_ros + zdepth_image_transport aques_talk @@ -56,6 +57,7 @@ python_twoauth slic voice_text + zdepth collada_urdf_jsk_patch From d8556146e8a57fc7d4ed09b9843b86e1cc4ca8e5 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 01:00:10 +0900 Subject: [PATCH 13/14] mv zdepth_image_transport from 3rdparty/ to / --- .../CMakeLists.txt | 0 .../include/zdepth_image_transport/zdepth_publisher.h | 0 .../include/zdepth_image_transport/zdepth_subscriber.h | 0 .../msg/ZDepthImage.msg | 0 .../zdepth_image_transport => zdepth_image_transport}/package.xml | 0 .../src/manifest.cpp | 0 .../src/test/zdepth_test.cpp | 0 .../src/zdepth_publisher.cpp | 0 .../src/zdepth_subscriber.cpp | 0 .../zdepth_plugins.xml | 0 10 files changed, 0 insertions(+), 0 deletions(-) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/CMakeLists.txt (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/include/zdepth_image_transport/zdepth_publisher.h (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/include/zdepth_image_transport/zdepth_subscriber.h (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/msg/ZDepthImage.msg (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/package.xml (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/manifest.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/test/zdepth_test.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/zdepth_publisher.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/zdepth_subscriber.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/zdepth_plugins.xml (100%) diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/zdepth_image_transport/CMakeLists.txt similarity index 100% rename from 3rdparty/zdepth_image_transport/CMakeLists.txt rename to zdepth_image_transport/CMakeLists.txt diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h b/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h similarity index 100% rename from 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h rename to zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h b/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h similarity index 100% rename from 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h rename to zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h diff --git a/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg b/zdepth_image_transport/msg/ZDepthImage.msg similarity index 100% rename from 3rdparty/zdepth_image_transport/msg/ZDepthImage.msg rename to zdepth_image_transport/msg/ZDepthImage.msg diff --git a/3rdparty/zdepth_image_transport/package.xml b/zdepth_image_transport/package.xml similarity index 100% rename from 3rdparty/zdepth_image_transport/package.xml rename to zdepth_image_transport/package.xml diff --git a/3rdparty/zdepth_image_transport/src/manifest.cpp b/zdepth_image_transport/src/manifest.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/manifest.cpp rename to zdepth_image_transport/src/manifest.cpp diff --git a/3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp b/zdepth_image_transport/src/test/zdepth_test.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp rename to zdepth_image_transport/src/test/zdepth_test.cpp diff --git a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp b/zdepth_image_transport/src/zdepth_publisher.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp rename to zdepth_image_transport/src/zdepth_publisher.cpp diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/zdepth_image_transport/src/zdepth_subscriber.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp rename to zdepth_image_transport/src/zdepth_subscriber.cpp diff --git a/3rdparty/zdepth_image_transport/zdepth_plugins.xml b/zdepth_image_transport/zdepth_plugins.xml similarity index 100% rename from 3rdparty/zdepth_image_transport/zdepth_plugins.xml rename to zdepth_image_transport/zdepth_plugins.xml From 4824c7caf309a61e40290ca6a1dc6ae110a0caa9 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 19:53:11 +0900 Subject: [PATCH 14/14] zdepth skip building kinetic --- 3rdparty/zdepth/CMakeLists.txt | 25 ++++++++------ zdepth_image_transport/CMakeLists.txt | 48 +++++++++++++++------------ 2 files changed, 40 insertions(+), 33 deletions(-) diff --git a/3rdparty/zdepth/CMakeLists.txt b/3rdparty/zdepth/CMakeLists.txt index 392eed536..52bb0d590 100644 --- a/3rdparty/zdepth/CMakeLists.txt +++ b/3rdparty/zdepth/CMakeLists.txt @@ -3,15 +3,18 @@ project(zdepth) include(ExternalProject) -ExternalProject_Add( - zdepth - GIT_REPOSITORY https://github.com/catid/Zdepth.git - GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 - GIT_SHALLOW TRUE - PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 - CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} - INSTALL_COMMAND echo "install" - ) +if("$ENV{ROS_DISTRO}" STRGREATER "kinetic") + ExternalProject_Add( + zdepth + GIT_REPOSITORY https://github.com/catid/Zdepth.git + GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 + GIT_SHALLOW TRUE + PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 + CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INSTALL_COMMAND echo "install" + ) - -install(CODE "execute_process(COMMAND make install WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/zdepth-prefix/src/zdepth-build)") + install(CODE "execute_process(COMMAND make install WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/zdepth-prefix/src/zdepth-build)") +else() + install(CODE "message(WARNING \"Skipping zdepth install because ROS is too old\")") +endif() diff --git a/zdepth_image_transport/CMakeLists.txt b/zdepth_image_transport/CMakeLists.txt index 227f6cf77..beea8481b 100644 --- a/zdepth_image_transport/CMakeLists.txt +++ b/zdepth_image_transport/CMakeLists.txt @@ -11,12 +11,14 @@ find_package(catkin REQUIRED std_msgs ) -find_library(zdepth_LIB zdepth PATH_SUFFIXES zdepth) -find_library(zstd_LIB zstd PATH_SUFFIXES zdepth) -set(zdepth_LIBRARIES ${zdepth_LIB} ${zstd_LIB}) -find_file(zdepth_INCLUDE zdepth.hpp PATH_SUFFIXES zdepth) -get_filename_component(zdepth_PATH ${zdepth_INCLUDE} DIRECTORY) -get_filename_component(zdepth_INCLUDE_DIRS ${zdepth_PATH} DIRECTORY) +if("$ENV{ROS_DISTRO}" STRGREATER "kinetic") + find_library(zdepth_LIB zdepth PATH_SUFFIXES zdepth) + find_library(zstd_LIB zstd PATH_SUFFIXES zdepth) + set(zdepth_LIBRARIES ${zdepth_LIB} ${zstd_LIB}) + find_file(zdepth_INCLUDE zdepth.hpp PATH_SUFFIXES zdepth) + get_filename_component(zdepth_PATH ${zdepth_INCLUDE} DIRECTORY) + get_filename_component(zdepth_INCLUDE_DIRS ${zdepth_PATH} DIRECTORY) +endif() add_message_files( DIRECTORY msg @@ -41,26 +43,28 @@ catkin_package( zdepth ) -include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) +if("$ENV{ROS_DISTRO}" STRGREATER "kinetic") + include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) -add_executable(zdepth_test src/test/zdepth_test.cpp) -target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + add_executable(zdepth_test src/test/zdepth_test.cpp) + target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) -add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) + add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) + target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) -install(TARGETS zdepth_test - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + install(TARGETS zdepth_test + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) +endif() install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}