From 539b692d42b4d16b7ded6bc74597b7a9282d1869 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 24 Jun 2026 17:42:12 +0700 Subject: [PATCH] first commit --- CMakeLists.txt | 295 +++++++++++++++++ .../depth_conversions.h | 102 ++++++ include/robot_depth_image_proc/depth_traits.h | 74 +++++ .../robot_depth_image_proc/point_cloud_xyz.h | 17 + .../ros_message_conversions.h | 132 ++++++++ launch/depth_image_proc_realsense.launch | 27 ++ launch/depth_image_proc_test.launch | 31 ++ package.xml | 53 +++ rviz/depth_image_proc_realsense.rviz | 67 ++++ rviz/depth_image_proc_test.rviz | 67 ++++ src/convert_metric.cpp | 140 ++++++++ src/crop_foremost.cpp | 142 ++++++++ src/depth_image_proc_node.cpp | 148 +++++++++ src/depth_image_proc_test_node.cpp | 211 ++++++++++++ src/disparity.cpp | 189 +++++++++++ src/point_cloud_xyz.cpp | 48 +++ src/register.cpp | 313 ++++++++++++++++++ test/test_point_cloud_xyz.cpp | 114 +++++++ 18 files changed, 2170 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 include/robot_depth_image_proc/depth_conversions.h create mode 100644 include/robot_depth_image_proc/depth_traits.h create mode 100644 include/robot_depth_image_proc/point_cloud_xyz.h create mode 100644 include/robot_depth_image_proc/ros_message_conversions.h create mode 100644 launch/depth_image_proc_realsense.launch create mode 100644 launch/depth_image_proc_test.launch create mode 100644 package.xml create mode 100644 rviz/depth_image_proc_realsense.rviz create mode 100644 rviz/depth_image_proc_test.rviz create mode 100644 src/convert_metric.cpp create mode 100755 src/crop_foremost.cpp create mode 100644 src/depth_image_proc_node.cpp create mode 100644 src/depth_image_proc_test_node.cpp create mode 100644 src/disparity.cpp create mode 100644 src/point_cloud_xyz.cpp create mode 100644 src/register.cpp create mode 100644 test/test_point_cloud_xyz.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6d41c02 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,295 @@ +cmake_minimum_required(VERSION 3.0.2) +project(robot_depth_image_proc VERSION 1.0.0 LANGUAGES CXX) + +# ======================================================== +# Detect build mode +# ======================================================== +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building robot_depth_image_proc with Catkin") +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building robot_depth_image_proc with Standalone CMake") +endif() + +# ======================================================== +# C++ Standard +# ======================================================== +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +add_compile_options(-Wall -Wextra -Wpedantic) + +# ======================================================== +# Common dependencies +# ======================================================== +find_package(Boost REQUIRED COMPONENTS thread) +find_package(Eigen3 REQUIRED) + +# ======================================================== +# Standalone mode +# ======================================================== +if(NOT BUILDING_WITH_CATKIN) + + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + # ⚠️ placeholder libraries + set(PACKAGES_DIR + cv_bridge + robot_sensor_msgs + robot_cpp + data_convert + robot_image_geometry + ) + + find_library(TF3_LIBRARY + NAMES tf3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + ) + + if(NOT TF3_LIBRARY) + message(FATAL_ERROR "❌ tf3 library not found") + endif() + +# ======================================================== +# Catkin mode +# ======================================================== +else() + + find_package(catkin REQUIRED COMPONENTS + cv_bridge + robot_sensor_msgs + robot_cpp + data_convert + robot_image_geometry + roscpp + sensor_msgs + std_msgs + tf2_ros + ) + + find_library(TF3_LIBRARY + NAMES tf3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + ) + + if(NOT TF3_LIBRARY) + message(FATAL_ERROR "❌ tf3 library not found") + endif() + + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + cv_bridge + robot_sensor_msgs + robot_cpp + data_convert + robot_image_geometry + roscpp + sensor_msgs + std_msgs + tf2_ros + DEPENDS Boost Eigen3 + ) + + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) + +endif() + +# ======================================================== +# Check headers +# ======================================================== +include(CheckIncludeFile) + +check_include_file(sys/time.h HAVE_SYS_TIME_H) + +if(HAVE_SYS_TIME_H) + add_definitions(-DHAVE_SYS_TIME_H) +endif() + +# ======================================================== +# Library +# ======================================================== +add_library(${PROJECT_NAME} SHARED + src/point_cloud_xyz.cpp +) + +# ======================================================== +# Catkin linking +# ======================================================== +if(BUILDING_WITH_CATKIN) + + add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} + ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ) + + target_link_libraries(${PROJECT_NAME} + + PUBLIC + ${catkin_LIBRARIES} + + PRIVATE + Boost::thread + Eigen3::Eigen + ${TF3_LIBRARY} + ) + +# ======================================================== +# Standalone linking +# ======================================================== +else() + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ) + + target_link_libraries(${PROJECT_NAME} + + PUBLIC + ${PACKAGES_DIR} + + PRIVATE + Boost::thread + Eigen3::Eigen + ${TF3_LIBRARY} + ) + + set_target_properties(${PROJECT_NAME} PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + BUILD_RPATH "${CMAKE_BINARY_DIR}" + INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" + ) + +endif() + +# ======================================================== +# Test node + unit tests +# ======================================================== +if(BUILDING_WITH_CATKIN) + + add_executable(depth_image_proc_test_node + src/depth_image_proc_test_node.cpp + ) + + add_dependencies(depth_image_proc_test_node + ${catkin_EXPORTED_TARGETS} + ) + + target_include_directories(depth_image_proc_test_node PRIVATE + include + ${catkin_INCLUDE_DIRS} + ) + + target_link_libraries(depth_image_proc_test_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + + add_executable(depth_image_proc_node + src/depth_image_proc_node.cpp + ) + + add_dependencies(depth_image_proc_node + ${catkin_EXPORTED_TARGETS} + ) + + target_include_directories(depth_image_proc_node PRIVATE + include + ${catkin_INCLUDE_DIRS} + ) + + target_link_libraries(depth_image_proc_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + + install(TARGETS depth_image_proc_test_node depth_image_proc_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + + if(CATKIN_ENABLE_TESTING) + find_package(GTest REQUIRED) + + catkin_add_gtest(test_point_cloud_xyz + test/test_point_cloud_xyz.cpp + ) + + if(TARGET test_point_cloud_xyz) + target_include_directories(test_point_cloud_xyz PRIVATE + include + ${catkin_INCLUDE_DIRS} + ) + + target_link_libraries(test_point_cloud_xyz + ${PROJECT_NAME} + ${catkin_LIBRARIES} + GTest::GTest + GTest::Main + ) + endif() + endif() + +endif() + +# ======================================================== +# Install +# ======================================================== +if(BUILDING_WITH_CATKIN) + + 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} + FILES_MATCHING PATTERN "*.h" + ) + +else() + + install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) + +endif() diff --git a/include/robot_depth_image_proc/depth_conversions.h b/include/robot_depth_image_proc/depth_conversions.h new file mode 100644 index 0000000..88c9ac0 --- /dev/null +++ b/include/robot_depth_image_proc/depth_conversions.h @@ -0,0 +1,102 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#ifndef DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS +#define DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS + +#include +#include +#include +#include +#include + +#include + +namespace depth_image_proc { + +typedef robot_sensor_msgs::PointCloud2 PointCloud; + +// Handles float or uint16 depths +template +void convert( + const robot_sensor_msgs::Image& depth_msg, + PointCloud& cloud_msg, + const image_geometry::PinholeCameraModel& model, + double range_max = 0.0) +{ + // Use correct principal point from calibration + float center_x = model.cx(); + float center_y = model.cy(); + + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + double unit_scaling = DepthTraits::toMeters( T(1) ); + float constant_x = unit_scaling / model.fx(); + float constant_y = unit_scaling / model.fy(); + float bad_point = std::numeric_limits::quiet_NaN(); + + robot_sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); + robot_sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + robot_sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); + const T* depth_row = reinterpret_cast(&depth_msg.data[0]); + int row_step = depth_msg.step / sizeof(T); + for (int v = 0; v < (int)cloud_msg.height; ++v, depth_row += row_step) + { + for (int u = 0; u < (int)cloud_msg.width; ++u, ++iter_x, ++iter_y, ++iter_z) + { + T depth = depth_row[u]; + + // Missing points denoted by NaNs + if (!DepthTraits::valid(depth)) + { + if (range_max != 0.0) + { + depth = DepthTraits::fromMeters(range_max); + } + else + { + *iter_x = *iter_y = *iter_z = bad_point; + continue; + } + } + + // Fill in XYZ + *iter_x = (u - center_x) * depth * constant_x; + *iter_y = (v - center_y) * depth * constant_y; + *iter_z = DepthTraits::toMeters(depth); + } + } +} + +} // namespace depth_image_proc + +#endif diff --git a/include/robot_depth_image_proc/depth_traits.h b/include/robot_depth_image_proc/depth_traits.h new file mode 100644 index 0000000..e365821 --- /dev/null +++ b/include/robot_depth_image_proc/depth_traits.h @@ -0,0 +1,74 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#ifndef DEPTH_IMAGE_PROC_DEPTH_TRAITS +#define DEPTH_IMAGE_PROC_DEPTH_TRAITS + +#include +#include +#include +#include +#include + +namespace depth_image_proc { + +// Encapsulate differences between processing float and uint16_t depths +template struct DepthTraits {}; + +template<> +struct DepthTraits +{ + static inline bool valid(uint16_t depth) { return depth != 0; } + static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm + static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; } + static inline void initializeBuffer(std::vector& buffer) {} // Do nothing - already zero-filled +}; + +template<> +struct DepthTraits +{ + static inline bool valid(float depth) { return std::isfinite(depth); } + static inline float toMeters(float depth) { return depth; } + static inline float fromMeters(float depth) { return depth; } + + static inline void initializeBuffer(std::vector& buffer) + { + float* start = reinterpret_cast(&buffer[0]); + float* end = reinterpret_cast(&buffer[0] + buffer.size()); + std::fill(start, end, std::numeric_limits::quiet_NaN()); + } +}; + +} // namespace depth_image_proc + +#endif diff --git a/include/robot_depth_image_proc/point_cloud_xyz.h b/include/robot_depth_image_proc/point_cloud_xyz.h new file mode 100644 index 0000000..a1e104e --- /dev/null +++ b/include/robot_depth_image_proc/point_cloud_xyz.h @@ -0,0 +1,17 @@ +#ifndef ROBOT_DEPTH_IMAGE_PROC_POINT_CLOUD_XYZ_H +#define ROBOT_DEPTH_IMAGE_PROC_POINT_CLOUD_XYZ_H + +#include +#include +#include + +namespace depth_image_proc +{ + +robot_sensor_msgs::PointCloud2 convertDepthToPointCloud( + const robot_sensor_msgs::Image& depth_msg, + const robot_sensor_msgs::CameraInfo& info_msg); + +} // namespace depth_image_proc + +#endif diff --git a/include/robot_depth_image_proc/ros_message_conversions.h b/include/robot_depth_image_proc/ros_message_conversions.h new file mode 100644 index 0000000..4004b0f --- /dev/null +++ b/include/robot_depth_image_proc/ros_message_conversions.h @@ -0,0 +1,132 @@ +#ifndef ROBOT_DEPTH_IMAGE_PROC_ROS_MESSAGE_CONVERSIONS_H +#define ROBOT_DEPTH_IMAGE_PROC_ROS_MESSAGE_CONVERSIONS_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +inline robot::Time toRobotTime(const ros::Time& stamp) +{ + robot::Time out; + out.sec = stamp.sec; + out.nsec = stamp.nsec; + return out; +} + +inline ros::Time toRosTime(const robot::Time& stamp) +{ + return ros::Time(stamp.sec, stamp.nsec); +} + +inline robot_std_msgs::Header toRobotHeader(const std_msgs::Header& header) +{ + robot_std_msgs::Header out; + out.seq = header.seq; + out.stamp = toRobotTime(header.stamp); + out.frame_id = header.frame_id; + return out; +} + +inline std_msgs::Header toStdHeader(const robot_std_msgs::Header& header) +{ + std_msgs::Header out; + out.seq = header.seq; + out.stamp = toRosTime(header.stamp); + out.frame_id = header.frame_id; + return out; +} + +inline robot_sensor_msgs::Image toRobotImage(const sensor_msgs::Image& image) +{ + robot_sensor_msgs::Image out; + out.header = toRobotHeader(image.header); + out.height = image.height; + out.width = image.width; + out.encoding = image.encoding; + out.is_bigendian = image.is_bigendian; + out.step = image.step; + out.data = image.data; + return out; +} + +inline sensor_msgs::Image toRosImage(const robot_sensor_msgs::Image& image) +{ + sensor_msgs::Image out; + out.header = toStdHeader(image.header); + out.height = image.height; + out.width = image.width; + out.encoding = image.encoding; + out.is_bigendian = image.is_bigendian; + out.step = image.step; + out.data = image.data; + return out; +} + +inline robot_sensor_msgs::CameraInfo toRobotCameraInfo(const sensor_msgs::CameraInfo& info) +{ + robot_sensor_msgs::CameraInfo out; + out.header = toRobotHeader(info.header); + out.height = info.height; + out.width = info.width; + out.distortion_model = info.distortion_model; + out.D = info.D; + for (size_t i = 0; i < info.K.size(); ++i) + { + out.K[i] = info.K[i]; + } + for (size_t i = 0; i < info.R.size(); ++i) + { + out.R[i] = info.R[i]; + } + for (size_t i = 0; i < info.P.size(); ++i) + { + out.P[i] = info.P[i]; + } + out.binning_x = info.binning_x; + out.binning_y = info.binning_y; + return out; +} + +inline sensor_msgs::PointField toRosPointField(const robot_sensor_msgs::PointField& field) +{ + sensor_msgs::PointField out; + out.name = field.name; + out.offset = field.offset; + out.datatype = field.datatype; + out.count = field.count; + return out; +} + +inline sensor_msgs::PointCloud2 toRosPointCloud(const robot_sensor_msgs::PointCloud2& cloud) +{ + sensor_msgs::PointCloud2 out; + out.header = toStdHeader(cloud.header); + out.height = cloud.height; + out.width = cloud.width; + out.fields.reserve(cloud.fields.size()); + for (const auto& field : cloud.fields) + { + out.fields.push_back(toRosPointField(field)); + } + out.is_bigendian = cloud.is_bigendian; + out.point_step = cloud.point_step; + out.row_step = cloud.row_step; + out.data = cloud.data; + out.is_dense = cloud.is_dense; + return out; +} + +} // namespace depth_image_proc + +#endif diff --git a/launch/depth_image_proc_realsense.launch b/launch/depth_image_proc_realsense.launch new file mode 100644 index 0000000..f819eaa --- /dev/null +++ b/launch/depth_image_proc_realsense.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/launch/depth_image_proc_test.launch b/launch/depth_image_proc_test.launch new file mode 100644 index 0000000..236c6f8 --- /dev/null +++ b/launch/depth_image_proc_test.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..699809c --- /dev/null +++ b/package.xml @@ -0,0 +1,53 @@ + + robot_depth_image_proc + 0.7.10 + + robot_depth_image_proc is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. robot_depth_image_proc + maintains the relationship between coordinate frames in a tree + structure buffered in time, and lets the user transform points, + vectors, etc between any two coordinate frames at any desired + point in time. + + Tully Foote + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/robot_depth_image_proc + + catkin + + libconsole-bridge-dev + libconsole-bridge-dev + + + + boost + cv_bridge + robot_image_geometry + eigen + robot_sensor_msgs + robot_cpp + tf3 + data_convert + roscpp + sensor_msgs + std_msgs + tf2_ros + + boost + cv_bridge + robot_image_geometry + eigen + robot_sensor_msgs + robot_cpp + tf3 + data_convert + roscpp + sensor_msgs + std_msgs + tf2_ros + rviz + diff --git a/rviz/depth_image_proc_realsense.rviz b/rviz/depth_image_proc_realsense.rviz new file mode 100644 index 0000000..5d4a291 --- /dev/null +++ b/rviz/depth_image_proc_realsense.rviz @@ -0,0 +1,67 @@ +Panels: + - Class: rviz/Displays + Name: Displays + - Class: rviz/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Class: rviz/Grid + Enabled: true + Name: Grid + Reference Frame: map + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: 0 + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Name: RealSense PointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Style: Points + Topic: /camera/depth/points + Use Fixed Frame: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/depth/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealSense Depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + - Class: rviz/MoveCamera + - Class: rviz/FocusCamera + Views: + Current: + Class: rviz/Orbit + Distance: 3 + Focal Point: + X: 0 + Y: 0 + Z: 1 + Name: Current View + Pitch: 0.4 + Yaw: 0.8 +Window Geometry: + Height: 800 + Width: 1200 diff --git a/rviz/depth_image_proc_test.rviz b/rviz/depth_image_proc_test.rviz new file mode 100644 index 0000000..9307d7e --- /dev/null +++ b/rviz/depth_image_proc_test.rviz @@ -0,0 +1,67 @@ +Panels: + - Class: rviz/Displays + Name: Displays + - Class: rviz/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Class: rviz/Grid + Enabled: true + Name: Grid + Reference Frame: map + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.5 + Min Value: 0.5 + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Name: Depth PointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Style: Points + Topic: /depth/points + Use Fixed Frame: true + - Class: rviz/Image + Enabled: true + Image Topic: /depth/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Depth Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + - Class: rviz/MoveCamera + - Class: rviz/FocusCamera + Views: + Current: + Class: rviz/Orbit + Distance: 4 + Focal Point: + X: 0 + Y: 0 + Z: 1 + Name: Current View + Pitch: 0.5 + Yaw: 0.8 +Window Geometry: + Height: 800 + Width: 1200 diff --git a/src/convert_metric.cpp b/src/convert_metric.cpp new file mode 100644 index 0000000..22552ae --- /dev/null +++ b/src/convert_metric.cpp @@ -0,0 +1,140 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#include +#include +#include +#include +#include + +namespace depth_image_proc { + +namespace enc = sensor_msgs::image_encodings; + +class ConvertMetricNodelet : public nodelet::Nodelet +{ + // Subscriptions + boost::shared_ptr it_; + image_transport::Subscriber sub_raw_; + + // Publications + boost::mutex connect_mutex_; + image_transport::Publisher pub_depth_; + + virtual void onInit(); + + void connectCb(); + + void depthCb(const sensor_msgs::ImageConstPtr& raw_msg); +}; + +void ConvertMetricNodelet::onInit() +{ + ros::NodeHandle& nh = getNodeHandle(); + it_.reset(new image_transport::ImageTransport(nh)); + + // Monitor whether anyone is subscribed to the output + image_transport::SubscriberStatusCallback connect_cb = boost::bind(&ConvertMetricNodelet::connectCb, this); + // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ + boost::lock_guard lock(connect_mutex_); + pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb); +} + +// Handles (un)subscribing when clients (un)subscribe +void ConvertMetricNodelet::connectCb() +{ + boost::lock_guard lock(connect_mutex_); + if (pub_depth_.getNumSubscribers() == 0) + { + sub_raw_.shutdown(); + } + else if (!sub_raw_) + { + image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); + sub_raw_ = it_->subscribe("image_raw", 1, &ConvertMetricNodelet::depthCb, this, hints); + } +} + +void ConvertMetricNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg) +{ + // Allocate new Image message + sensor_msgs::ImagePtr depth_msg( new sensor_msgs::Image ); + depth_msg->header = raw_msg->header; + depth_msg->height = raw_msg->height; + depth_msg->width = raw_msg->width; + + // Set data, encoding and step after converting the metric. + if (raw_msg->encoding == enc::TYPE_16UC1) + { + depth_msg->encoding = enc::TYPE_32FC1; + depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8); + depth_msg->data.resize(depth_msg->height * depth_msg->step); + // Fill in the depth image data, converting mm to m + float bad_point = std::numeric_limits::quiet_NaN (); + const uint16_t* raw_data = reinterpret_cast(&raw_msg->data[0]); + float* depth_data = reinterpret_cast(&depth_msg->data[0]); + for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index) + { + uint16_t raw = raw_data[index]; + depth_data[index] = (raw == 0) ? bad_point : (float)raw * 0.001f; + } + } + else if (raw_msg->encoding == enc::TYPE_32FC1) + { + depth_msg->encoding = enc::TYPE_16UC1; + depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8); + depth_msg->data.resize(depth_msg->height * depth_msg->step); + // Fill in the depth image data, converting m to mm + uint16_t bad_point = 0; + const float* raw_data = reinterpret_cast(&raw_msg->data[0]); + uint16_t* depth_data = reinterpret_cast(&depth_msg->data[0]); + for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index) + { + float raw = raw_data[index]; + depth_data[index] = std::isnan(raw) ? bad_point : (uint16_t)(raw * 1000); + } + } + else + { + ROS_ERROR("Unsupported image conversion from %s.", raw_msg->encoding.c_str()); + return; + } + + pub_depth_.publish(depth_msg); +} + +} // namespace depth_image_proc + +// Register as nodelet +#include +PLUGINLIB_EXPORT_CLASS(depth_image_proc::ConvertMetricNodelet,nodelet::Nodelet); diff --git a/src/crop_foremost.cpp b/src/crop_foremost.cpp new file mode 100755 index 0000000..f4cbac8 --- /dev/null +++ b/src/crop_foremost.cpp @@ -0,0 +1,142 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +//#include +#include +#include +#include +#include +#include + +namespace depth_image_proc { + +namespace enc = sensor_msgs::image_encodings; + +class CropForemostNodelet : public nodelet::Nodelet +{ + // Subscriptions + boost::shared_ptr it_; + image_transport::Subscriber sub_raw_; + + // Publications + boost::mutex connect_mutex_; + image_transport::Publisher pub_depth_; + + virtual void onInit(); + + void connectCb(); + + void depthCb(const sensor_msgs::ImageConstPtr& raw_msg); + + double distance_; +}; + +void CropForemostNodelet::onInit() +{ + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& private_nh = getPrivateNodeHandle(); + private_nh.getParam("distance", distance_); + it_.reset(new image_transport::ImageTransport(nh)); + + // Monitor whether anyone is subscribed to the output + image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropForemostNodelet::connectCb, this); + // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ + boost::lock_guard lock(connect_mutex_); + pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb); +} + +// Handles (un)subscribing when clients (un)subscribe +void CropForemostNodelet::connectCb() +{ + boost::lock_guard lock(connect_mutex_); + if (pub_depth_.getNumSubscribers() == 0) + { + sub_raw_.shutdown(); + } + else if (!sub_raw_) + { + image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); + sub_raw_ = it_->subscribe("image_raw", 1, &CropForemostNodelet::depthCb, this, hints); + } +} + +void CropForemostNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg) +{ + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(raw_msg); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Check the number of channels + if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){ + NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str()); + return; + } + + // search the min value without invalid value "0" + double minVal; + cv::minMaxIdx(cv_ptr->image, &minVal, 0, 0, 0, cv_ptr->image != 0); + + int imtype = cv_bridge::getCvType(raw_msg->encoding); + switch (imtype){ + case CV_8UC1: + case CV_8SC1: + case CV_32F: + cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 0, CV_THRESH_TOZERO_INV); + break; + case CV_16UC1: + case CV_16SC1: + case CV_32SC1: + case CV_64F: + // 8 bit or 32 bit floating array is required to use cv::threshold + cv_ptr->image.convertTo(cv_ptr->image, CV_32F); + cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 1, CV_THRESH_TOZERO_INV); + + cv_ptr->image.convertTo(cv_ptr->image, imtype); + break; + } + + pub_depth_.publish(cv_ptr->toImageMsg()); +} + +} // namespace depth_image_proc + +// Register as nodelet +#include +PLUGINLIB_EXPORT_CLASS(depth_image_proc::CropForemostNodelet,nodelet::Nodelet); diff --git a/src/depth_image_proc_node.cpp b/src/depth_image_proc_node.cpp new file mode 100644 index 0000000..ce65a70 --- /dev/null +++ b/src/depth_image_proc_node.cpp @@ -0,0 +1,148 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +class DepthImageProcNode +{ +public: + DepthImageProcNode(ros::NodeHandle& nh, ros::NodeHandle& pnh) + { + std::string depth_topic; + std::string camera_info_topic; + std::string cloud_topic; + + pnh.param("depth_topic", depth_topic, std::string("/camera/depth/image_raw")); + pnh.param("camera_info_topic", camera_info_topic, std::string("/camera/depth/camera_info")); + pnh.param("cloud_topic", cloud_topic, std::string("/camera/depth/points")); + pnh.param("fixed_frame", fixed_frame_, std::string("map")); + pnh.param("publish_tf", publish_tf_, true); + + cloud_pub_ = nh.advertise(cloud_topic, 1); + + camera_info_sub_ = nh.subscribe( + camera_info_topic, + 1, + &DepthImageProcNode::cameraInfoCallback, + this); + + depth_sub_ = nh.subscribe( + depth_topic, + 1, + &DepthImageProcNode::depthCallback, + this); + + if (publish_tf_) + { + publishStaticTransform(); + } + + ROS_INFO( + "depth_image_proc listening on [%s] + [%s], publishing [%s]", + depth_topic.c_str(), + camera_info_topic.c_str(), + cloud_topic.c_str()); + } + +private: + void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg) + { + std::lock_guard lock(mutex_); + camera_info_ = depth_image_proc::toRobotCameraInfo(*msg); + has_camera_info_ = true; + + if (publish_tf_ && !frame_id_.empty() && frame_id_ != msg->header.frame_id) + { + frame_id_ = msg->header.frame_id; + publishStaticTransform(); + } + else if (frame_id_.empty()) + { + frame_id_ = msg->header.frame_id; + } + } + + void depthCallback(const sensor_msgs::ImageConstPtr& msg) + { + robot_sensor_msgs::CameraInfo camera_info; + { + std::lock_guard lock(mutex_); + if (!has_camera_info_) + { + ROS_WARN_THROTTLE(5.0, "Waiting for camera_info before converting depth image"); + return; + } + camera_info = camera_info_; + } + + if (msg->encoding != "16UC1" && msg->encoding != "mono16" && msg->encoding != "32FC1") + { + ROS_ERROR_THROTTLE( + 5.0, + "Unsupported depth encoding [%s], expected 16UC1/mono16/32FC1", + msg->encoding.c_str()); + return; + } + + const robot_sensor_msgs::Image depth = depth_image_proc::toRobotImage(*msg); + const robot_sensor_msgs::PointCloud2 cloud = + depth_image_proc::convertDepthToPointCloud(depth, camera_info); + + if (cloud.width == 0 || cloud.height == 0) + { + ROS_ERROR_THROTTLE(5.0, "depth_image_proc conversion returned an empty point cloud"); + return; + } + + sensor_msgs::PointCloud2 ros_cloud = depth_image_proc::toRosPointCloud(cloud); + ros_cloud.header.stamp = msg->header.stamp; + ros_cloud.header.frame_id = msg->header.frame_id; + cloud_pub_.publish(ros_cloud); + } + + void publishStaticTransform() + { + if (frame_id_.empty()) + { + frame_id_ = "camera_depth_optical_frame"; + } + + geometry_msgs::TransformStamped transform; + transform.header.stamp = ros::Time::now(); + transform.header.frame_id = fixed_frame_; + transform.child_frame_id = frame_id_; + transform.transform.rotation.w = 1.0; + static_broadcaster_.sendTransform(transform); + } + + std::mutex mutex_; + robot_sensor_msgs::CameraInfo camera_info_; + bool has_camera_info_{false}; + + std::string fixed_frame_; + std::string frame_id_; + bool publish_tf_{true}; + + ros::Subscriber depth_sub_; + ros::Subscriber camera_info_sub_; + ros::Publisher cloud_pub_; + tf2_ros::StaticTransformBroadcaster static_broadcaster_; +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "depth_image_proc_node"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + DepthImageProcNode node(nh, pnh); + ros::spin(); + return 0; +} diff --git a/src/depth_image_proc_test_node.cpp b/src/depth_image_proc_test_node.cpp new file mode 100644 index 0000000..70cda7a --- /dev/null +++ b/src/depth_image_proc_test_node.cpp @@ -0,0 +1,211 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ + +constexpr double kFx = 525.0; +constexpr double kFy = 525.0; +constexpr double kCx = 319.5; +constexpr double kCy = 239.5; + +robot_sensor_msgs::CameraInfo makeCameraInfo( + uint32_t width, + uint32_t height, + const std::string& frame_id) +{ + robot_sensor_msgs::CameraInfo info; + info.header.frame_id = frame_id; + info.height = height; + info.width = width; + info.distortion_model = "plumb_bob"; + info.D = {0.0, 0.0, 0.0, 0.0, 0.0}; + + info.K = { + kFx, 0.0, kCx, + 0.0, kFy, kCy, + 0.0, 0.0, 1.0}; + + info.R = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0}; + + info.P = { + kFx, 0.0, kCx, 0.0, + 0.0, kFy, kCy, 0.0, + 0.0, 0.0, 1.0, 0.0}; + + return info; +} + +robot_sensor_msgs::Image makeSyntheticDepthImage( + uint32_t width, + uint32_t height, + const std::string& frame_id) +{ + robot_sensor_msgs::Image image; + image.header.frame_id = frame_id; + image.height = height; + image.width = width; + image.encoding = robot_sensor_msgs::image_encodings::TYPE_16UC1; + image.is_bigendian = false; + image.step = width * sizeof(uint16_t); + image.data.resize(static_cast(height) * image.step, 0); + + const uint32_t box_u0 = width / 3; + const uint32_t box_u1 = 2 * width / 3; + const uint32_t box_v0 = height / 3; + const uint32_t box_v1 = 2 * height / 3; + + auto* depth = reinterpret_cast(image.data.data()); + for (uint32_t v = 0; v < height; ++v) + { + for (uint32_t u = 0; u < width; ++u) + { + const bool in_box = + u >= box_u0 && u < box_u1 && v >= box_v0 && v < box_v1; + + const double z_m = in_box ? 1.0 : 2.0; + const double radial = std::hypot( + static_cast(u) - kCx, + static_cast(v) - kCy); + const double ripple_m = 0.05 * std::sin(radial * 0.05); + const uint16_t depth_mm = static_cast((z_m + ripple_m) * 1000.0); + + depth[v * width + u] = depth_mm; + } + } + + return image; +} + +} // namespace + +class DepthImageProcTestNode +{ +public: + DepthImageProcTestNode(ros::NodeHandle& nh, ros::NodeHandle& pnh) + { + pnh.param("frame_id", frame_id_, std::string("camera_depth_optical_frame")); + pnh.param("fixed_frame", fixed_frame_, std::string("map")); + pnh.param("publish_rate", publish_rate_, 10.0); + pnh.param("width", width_, 640); + pnh.param("height", height_, 480); + + cloud_pub_ = nh.advertise("depth/points", 1); + depth_pub_ = nh.advertise("depth/image", 1); + camera_info_pub_ = nh.advertise("depth/camera_info", 1, true); + + camera_info_ = makeCameraInfo( + static_cast(width_), + static_cast(height_), + frame_id_); + publishStaticTransform(); + + timer_ = nh.createTimer( + ros::Duration(1.0 / publish_rate_), + &DepthImageProcTestNode::publishCallback, + this); + } + +private: + void publishStaticTransform() + { + geometry_msgs::TransformStamped transform; + transform.header.stamp = ros::Time::now(); + transform.header.frame_id = fixed_frame_; + transform.child_frame_id = frame_id_; + transform.transform.rotation.w = 1.0; + static_broadcaster_.sendTransform(transform); + } + + void publishCallback(const ros::TimerEvent&) + { + const ros::Time stamp = ros::Time::now(); + + robot_sensor_msgs::Image depth = makeSyntheticDepthImage( + static_cast(width_), + static_cast(height_), + frame_id_); + depth.header.stamp.sec = stamp.sec; + depth.header.stamp.nsec = stamp.nsec; + + camera_info_.header.stamp = depth.header.stamp; + + const robot_sensor_msgs::PointCloud2 cloud = + depth_image_proc::convertDepthToPointCloud(depth, camera_info_); + + if (cloud.width == 0 || cloud.height == 0) + { + ROS_ERROR_THROTTLE(5.0, "depth_image_proc conversion returned an empty point cloud"); + return; + } + + sensor_msgs::PointCloud2 ros_cloud = depth_image_proc::toRosPointCloud(cloud); + ros_cloud.header.stamp = stamp; + + sensor_msgs::Image ros_depth = depth_image_proc::toRosImage(depth); + ros_depth.header.stamp = stamp; + + sensor_msgs::CameraInfo ros_info; + ros_info.header = ros_depth.header; + ros_info.height = camera_info_.height; + ros_info.width = camera_info_.width; + ros_info.distortion_model = camera_info_.distortion_model; + ros_info.D = camera_info_.D; + for (size_t i = 0; i < camera_info_.K.size(); ++i) + { + ros_info.K[i] = camera_info_.K[i]; + } + for (size_t i = 0; i < camera_info_.R.size(); ++i) + { + ros_info.R[i] = camera_info_.R[i]; + } + for (size_t i = 0; i < camera_info_.P.size(); ++i) + { + ros_info.P[i] = camera_info_.P[i]; + } + + cloud_pub_.publish(ros_cloud); + depth_pub_.publish(ros_depth); + camera_info_pub_.publish(ros_info); + } + + std::string frame_id_; + std::string fixed_frame_; + double publish_rate_{10.0}; + int width_{640}; + int height_{480}; + + robot_sensor_msgs::CameraInfo camera_info_; + ros::Publisher cloud_pub_; + ros::Publisher depth_pub_; + ros::Publisher camera_info_pub_; + ros::Timer timer_; + tf2_ros::StaticTransformBroadcaster static_broadcaster_; +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "depth_image_proc_test_node"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + DepthImageProcTestNode node(nh, pnh); + ROS_INFO("Publishing synthetic depth cloud on /depth/points for RViz"); + ros::spin(); + return 0; +} diff --git a/src/disparity.cpp b/src/disparity.cpp new file mode 100644 index 0000000..83b0cf4 --- /dev/null +++ b/src/disparity.cpp @@ -0,0 +1,189 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#include +#if ((BOOST_VERSION / 100) % 1000) >= 53 +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc { + +namespace enc = sensor_msgs::image_encodings; + +class DisparityNodelet : public nodelet::Nodelet +{ + boost::shared_ptr left_it_; + ros::NodeHandlePtr right_nh_; + image_transport::SubscriberFilter sub_depth_image_; + message_filters::Subscriber sub_info_; + typedef message_filters::TimeSynchronizer Sync; + boost::shared_ptr sync_; + + boost::mutex connect_mutex_; + ros::Publisher pub_disparity_; + double min_range_; + double max_range_; + double delta_d_; + + virtual void onInit(); + + void connectCb(); + + void depthCb(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg); + + template + void convert(const sensor_msgs::ImageConstPtr& depth_msg, + stereo_msgs::DisparityImagePtr& disp_msg); +}; + +void DisparityNodelet::onInit() +{ + ros::NodeHandle &nh = getNodeHandle(); + ros::NodeHandle &private_nh = getPrivateNodeHandle(); + ros::NodeHandle left_nh(nh, "left"); + left_it_.reset(new image_transport::ImageTransport(left_nh)); + right_nh_.reset( new ros::NodeHandle(nh, "right") ); + + // Read parameters + int queue_size; + private_nh.param("queue_size", queue_size, 5); + private_nh.param("min_range", min_range_, 0.0); + private_nh.param("max_range", max_range_, std::numeric_limits::infinity()); + private_nh.param("delta_d", delta_d_, 0.125); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + sync_.reset( new Sync(sub_depth_image_, sub_info_, queue_size) ); + sync_->registerCallback(boost::bind(&DisparityNodelet::depthCb, this, boost::placeholders::_1, boost::placeholders::_2)); + + // Monitor whether anyone is subscribed to the output + ros::SubscriberStatusCallback connect_cb = boost::bind(&DisparityNodelet::connectCb, this); + // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_ + boost::lock_guard lock(connect_mutex_); + pub_disparity_ = left_nh.advertise("disparity", 1, connect_cb, connect_cb); +} + +// Handles (un)subscribing when clients (un)subscribe +void DisparityNodelet::connectCb() +{ + boost::lock_guard lock(connect_mutex_); + if (pub_disparity_.getNumSubscribers() == 0) + { + sub_depth_image_.unsubscribe(); + sub_info_ .unsubscribe(); + } + else if (!sub_depth_image_.getSubscriber()) + { + image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); + sub_depth_image_.subscribe(*left_it_, "image_rect", 1, hints); + sub_info_.subscribe(*right_nh_, "camera_info", 1); + } +} + +void DisparityNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg) +{ + // Allocate new DisparityImage message + stereo_msgs::DisparityImagePtr disp_msg( new stereo_msgs::DisparityImage ); + disp_msg->header = depth_msg->header; + disp_msg->image.header = disp_msg->header; + disp_msg->image.encoding = enc::TYPE_32FC1; + disp_msg->image.height = depth_msg->height; + disp_msg->image.width = depth_msg->width; + disp_msg->image.step = disp_msg->image.width * sizeof (float); + disp_msg->image.data.resize( disp_msg->image.height * disp_msg->image.step, 0.0f ); + double fx = info_msg->P[0]; + disp_msg->T = -info_msg->P[3] / fx; + disp_msg->f = fx; + // Remaining fields depend on device characteristics, so rely on user input + disp_msg->min_disparity = disp_msg->f * disp_msg->T / max_range_; + disp_msg->max_disparity = disp_msg->f * disp_msg->T / min_range_; + disp_msg->delta_d = delta_d_; + + if (depth_msg->encoding == enc::TYPE_16UC1) + { + convert(depth_msg, disp_msg); + } + else if (depth_msg->encoding == enc::TYPE_32FC1) + { + convert(depth_msg, disp_msg); + } + else + { + NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + + pub_disparity_.publish(disp_msg); +} + +template +void DisparityNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, + stereo_msgs::DisparityImagePtr& disp_msg) +{ + // For each depth Z, disparity d = fT / Z + float unit_scaling = DepthTraits::toMeters( T(1) ); + float constant = disp_msg->f * disp_msg->T / unit_scaling; + + const T* depth_row = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + float* disp_data = reinterpret_cast(&disp_msg->image.data[0]); + for (int v = 0; v < (int)depth_msg->height; ++v) + { + for (int u = 0; u < (int)depth_msg->width; ++u) + { + T depth = depth_row[u]; + if (DepthTraits::valid(depth)) + *disp_data = constant / depth; + ++disp_data; + } + + depth_row += row_step; + } +} + +} // namespace depth_image_proc + +// Register as nodelet +#include +PLUGINLIB_EXPORT_CLASS(depth_image_proc::DisparityNodelet,nodelet::Nodelet); diff --git a/src/point_cloud_xyz.cpp b/src/point_cloud_xyz.cpp new file mode 100644 index 0000000..f376803 --- /dev/null +++ b/src/point_cloud_xyz.cpp @@ -0,0 +1,48 @@ +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = robot_sensor_msgs::image_encodings; + +robot_sensor_msgs::PointCloud2 convertDepthToPointCloud( + const robot_sensor_msgs::Image& depth_msg, + const robot_sensor_msgs::CameraInfo& info_msg) +{ + robot_sensor_msgs::PointCloud2 cloud_msg; + cloud_msg.header = depth_msg.header; + cloud_msg.height = depth_msg.height; + cloud_msg.width = depth_msg.width; + cloud_msg.is_dense = false; + cloud_msg.is_bigendian = false; + + robot_sensor_msgs::PointCloud2Modifier pcd_modifier(cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(1, "xyz"); + + image_geometry::PinholeCameraModel model; + model.fromCameraInfo(info_msg); + + if (depth_msg.encoding == enc::TYPE_16UC1 || depth_msg.encoding == enc::MONO16) + { + convert(depth_msg, cloud_msg, model); + } + else if (depth_msg.encoding == enc::TYPE_32FC1) + { + convert(depth_msg, cloud_msg, model); + } + else + { + robot::log_error_throttle( + 5, "Depth image has unsupported encoding [%s]", depth_msg.encoding.c_str()); + return robot_sensor_msgs::PointCloud2(); + } + + return cloud_msg; +} + +} // namespace depth_image_proc diff --git a/src/register.cpp b/src/register.cpp new file mode 100644 index 0000000..412feb3 --- /dev/null +++ b/src/register.cpp @@ -0,0 +1,313 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc { + +using namespace message_filters::sync_policies; +namespace enc = sensor_msgs::image_encodings; + +class RegisterNodelet : public nodelet::Nodelet +{ + ros::NodeHandlePtr nh_depth_, nh_rgb_; + boost::shared_ptr it_depth_; + + // Subscriptions + image_transport::SubscriberFilter sub_depth_image_; + message_filters::Subscriber sub_depth_info_, sub_rgb_info_; + boost::shared_ptr tf_buffer_; + boost::shared_ptr tf_; + typedef ApproximateTime SyncPolicy; + typedef message_filters::Synchronizer Synchronizer; + boost::shared_ptr sync_; + + // Publications + boost::mutex connect_mutex_; + image_transport::CameraPublisher pub_registered_; + + image_geometry::PinholeCameraModel depth_model_, rgb_model_; + + // Parameters + bool fill_upsampling_holes_; // fills holes which occur due to upsampling by scaling each pixel to the target image scale (only takes effect on upsampling) + bool use_rgb_timestamp_; // use source time stamp from RGB camera + + virtual void onInit(); + + void connectCb(); + + void imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg, + const sensor_msgs::CameraInfoConstPtr& depth_info_msg, + const sensor_msgs::CameraInfoConstPtr& rgb_info_msg); + + template + void convert(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::ImagePtr& registered_msg, + const Eigen::Affine3d& depth_to_rgb); +}; + +void RegisterNodelet::onInit() +{ + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& private_nh = getPrivateNodeHandle(); + nh_depth_.reset( new ros::NodeHandle(nh, "depth") ); + nh_rgb_.reset( new ros::NodeHandle(nh, "rgb") ); + it_depth_.reset( new image_transport::ImageTransport(*nh_depth_) ); + tf_buffer_.reset( new tf2_ros::Buffer ); + tf_.reset( new tf2_ros::TransformListener(*tf_buffer_) ); + + // Read parameters + int queue_size; + private_nh.param("queue_size", queue_size, 5); + private_nh.param("fill_upsampling_holes", fill_upsampling_holes_, false); + private_nh.param("use_rgb_timestamp", use_rgb_timestamp_, false); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image_, sub_depth_info_, sub_rgb_info_) ); + sync_->registerCallback(boost::bind(&RegisterNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); + + // Monitor whether anyone is subscribed to the output + image_transport::ImageTransport it_depth_reg(ros::NodeHandle(nh, "depth_registered")); + image_transport::SubscriberStatusCallback image_connect_cb = boost::bind(&RegisterNodelet::connectCb, this); + ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNodelet::connectCb, this); + // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_ + boost::lock_guard lock(connect_mutex_); + pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1, + image_connect_cb, image_connect_cb, + info_connect_cb, info_connect_cb); +} + +// Handles (un)subscribing when clients (un)subscribe +void RegisterNodelet::connectCb() +{ + boost::lock_guard lock(connect_mutex_); + if (pub_registered_.getNumSubscribers() == 0) + { + sub_depth_image_.unsubscribe(); + sub_depth_info_ .unsubscribe(); + sub_rgb_info_ .unsubscribe(); + } + else if (!sub_depth_image_.getSubscriber()) + { + image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); + sub_depth_image_.subscribe(*it_depth_, "image_rect", 1, hints); + sub_depth_info_ .subscribe(*nh_depth_, "camera_info", 1); + sub_rgb_info_ .subscribe(*nh_rgb_, "camera_info", 1); + } +} + +void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg, + const sensor_msgs::CameraInfoConstPtr& depth_info_msg, + const sensor_msgs::CameraInfoConstPtr& rgb_info_msg) +{ + // Update camera models - these take binning & ROI into account + depth_model_.fromCameraInfo(depth_info_msg); + rgb_model_ .fromCameraInfo(rgb_info_msg); + + // Query tf2 for transform from (X,Y,Z) in depth camera frame to RGB camera frame + Eigen::Affine3d depth_to_rgb; + try + { + geometry_msgs::TransformStamped transform = tf_buffer_->lookupTransform ( + rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id, + depth_info_msg->header.stamp); + + tf::transformMsgToEigen(transform.transform, depth_to_rgb); + } + catch (tf2::TransformException& ex) + { + NODELET_WARN_THROTTLE(2, "TF2 exception:\n%s", ex.what()); + return; + /// @todo Can take on order of a minute to register a disconnect callback when we + /// don't call publish() in this cb. What's going on roscpp? + } + + // Allocate registered depth image + sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image ); + registered_msg->header.stamp = use_rgb_timestamp_ ? rgb_info_msg->header.stamp : depth_image_msg->header.stamp; + registered_msg->header.frame_id = rgb_info_msg->header.frame_id; + registered_msg->encoding = depth_image_msg->encoding; + + cv::Size resolution = rgb_model_.reducedResolution(); + registered_msg->height = resolution.height; + registered_msg->width = resolution.width; + // step and data set in convert(), depend on depth data type + + if (depth_image_msg->encoding == enc::TYPE_16UC1) + { + convert(depth_image_msg, registered_msg, depth_to_rgb); + } + else if (depth_image_msg->encoding == enc::TYPE_32FC1) + { + convert(depth_image_msg, registered_msg, depth_to_rgb); + } + else + { + NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image_msg->encoding.c_str()); + return; + } + + // Registered camera info is the same as the RGB info, but uses the depth timestamp + sensor_msgs::CameraInfoPtr registered_info_msg( new sensor_msgs::CameraInfo(*rgb_info_msg) ); + registered_info_msg->header.stamp = registered_msg->header.stamp; + + pub_registered_.publish(registered_msg, registered_info_msg); +} + +template +void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::ImagePtr& registered_msg, + const Eigen::Affine3d& depth_to_rgb) +{ + // Allocate memory for registered depth image + registered_msg->step = registered_msg->width * sizeof(T); + registered_msg->data.resize( registered_msg->height * registered_msg->step ); + // data is already zero-filled in the uint16 case, but for floats we want to initialize everything to NaN. + DepthTraits::initializeBuffer(registered_msg->data); + + // Extract all the parameters we need + double inv_depth_fx = 1.0 / depth_model_.fx(); + double inv_depth_fy = 1.0 / depth_model_.fy(); + double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy(); + double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty(); + double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy(); + double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy(); + double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty(); + + // Transform the depth values into the RGB frame + /// @todo When RGB is higher res, interpolate by rasterizing depth triangles onto the registered image + const T* depth_row = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + T* registered_data = reinterpret_cast(®istered_msg->data[0]); + int raw_index = 0; + for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step) + { + for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index) + { + T raw_depth = depth_row[u]; + if (!DepthTraits::valid(raw_depth)) + continue; + + double depth = DepthTraits::toMeters(raw_depth); + + if (fill_upsampling_holes_ == false) + { + /// @todo Combine all operations into one matrix multiply on (u,v,d) + // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame + Eigen::Vector4d xyz_depth; + xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx, + ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy, + depth, + 1; + + // Transform to RGB camera frame + Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth; + + // Project to (u,v) in RGB image + double inv_Z = 1.0 / xyz_rgb.z(); + int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5; + int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5; + + if (u_rgb < 0 || u_rgb >= (int)registered_msg->width || + v_rgb < 0 || v_rgb >= (int)registered_msg->height) + continue; + + T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb]; + T new_depth = DepthTraits::fromMeters(xyz_rgb.z()); + // Validity and Z-buffer checks + if (!DepthTraits::valid(reg_depth) || reg_depth > new_depth) + reg_depth = new_depth; + } + else + { + // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame + Eigen::Vector4d xyz_depth_1, xyz_depth_2; + xyz_depth_1 << ((u-0.5f - depth_cx)*depth - depth_Tx) * inv_depth_fx, + ((v-0.5f - depth_cy)*depth - depth_Ty) * inv_depth_fy, + depth, + 1; + xyz_depth_2 << ((u+0.5f - depth_cx)*depth - depth_Tx) * inv_depth_fx, + ((v+0.5f - depth_cy)*depth - depth_Ty) * inv_depth_fy, + depth, + 1; + + // Transform to RGB camera frame + Eigen::Vector4d xyz_rgb_1 = depth_to_rgb * xyz_depth_1; + Eigen::Vector4d xyz_rgb_2 = depth_to_rgb * xyz_depth_2; + + // Project to (u,v) in RGB image + double inv_Z = 1.0 / xyz_rgb_1.z(); + int u_rgb_1 = (rgb_fx*xyz_rgb_1.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5; + int v_rgb_1 = (rgb_fy*xyz_rgb_1.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5; + inv_Z = 1.0 / xyz_rgb_2.z(); + int u_rgb_2 = (rgb_fx*xyz_rgb_2.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5; + int v_rgb_2 = (rgb_fy*xyz_rgb_2.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5; + + if (u_rgb_1 < 0 || u_rgb_2 >= (int)registered_msg->width || + v_rgb_1 < 0 || v_rgb_2 >= (int)registered_msg->height) + continue; + + for (int nv=v_rgb_1; nv<=v_rgb_2; ++nv) + { + for (int nu=u_rgb_1; nu<=u_rgb_2; ++nu) + { + T& reg_depth = registered_data[nv*registered_msg->width + nu]; + T new_depth = DepthTraits::fromMeters(0.5*(xyz_rgb_1.z()+xyz_rgb_2.z())); + // Validity and Z-buffer checks + if (!DepthTraits::valid(reg_depth) || reg_depth > new_depth) + reg_depth = new_depth; + } + } + } + } + } +} + +} // namespace depth_image_proc + +// Register as nodelet +#include +PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet,nodelet::Nodelet); diff --git a/test/test_point_cloud_xyz.cpp b/test/test_point_cloud_xyz.cpp new file mode 100644 index 0000000..30afe19 --- /dev/null +++ b/test/test_point_cloud_xyz.cpp @@ -0,0 +1,114 @@ +#include + +#include +#include + +#include +#include +#include + +namespace +{ + +robot_sensor_msgs::CameraInfo makeCameraInfo(uint32_t width, uint32_t height) +{ + const double cx = (static_cast(width) - 1.0) * 0.5; + const double cy = (static_cast(height) - 1.0) * 0.5; + + robot_sensor_msgs::CameraInfo info; + info.height = height; + info.width = width; + info.distortion_model = "plumb_bob"; + info.D = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.K = { + 525.0, 0.0, cx, + 0.0, 525.0, cy, + 0.0, 0.0, 1.0}; + info.R = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0}; + info.P = { + 525.0, 0.0, cx, 0.0, + 0.0, 525.0, cy, 0.0, + 0.0, 0.0, 1.0, 0.0}; + return info; +} + +robot_sensor_msgs::Image makeFlatDepthImage( + uint32_t width, + uint32_t height, + uint16_t depth_mm) +{ + robot_sensor_msgs::Image image; + image.height = height; + image.width = width; + image.encoding = robot_sensor_msgs::image_encodings::TYPE_16UC1; + image.is_bigendian = false; + image.step = width * sizeof(uint16_t); + image.data.resize(static_cast(height) * image.step); + + auto* depth = reinterpret_cast(image.data.data()); + for (size_t i = 0; i < width * height; ++i) + { + depth[i] = depth_mm; + } + return image; +} + +} // namespace + +TEST(PointCloudXyz, ConvertsFlatDepthImage) +{ + const uint32_t width = 3; + const uint32_t height = 3; + const uint16_t depth_mm = 2000; + + const robot_sensor_msgs::Image depth = makeFlatDepthImage(width, height, depth_mm); + const robot_sensor_msgs::CameraInfo info = makeCameraInfo(width, height); + + const robot_sensor_msgs::PointCloud2 cloud = + depth_image_proc::convertDepthToPointCloud(depth, info); + + EXPECT_EQ(cloud.width, width); + EXPECT_EQ(cloud.height, height); + ASSERT_FALSE(cloud.data.empty()); + + robot_sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); + robot_sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); + robot_sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); + + const float expected_z = static_cast(depth_mm) * 0.001f; + const size_t center_index = (height / 2) * width + (width / 2); + size_t index = 0; + + for (size_t i = 0; i < width * height; ++i, ++iter_x, ++iter_y, ++iter_z, ++index) + { + EXPECT_NEAR(*iter_z, expected_z, 1e-3f); + + if (index == center_index) + { + EXPECT_NEAR(*iter_x, 0.0f, 1e-3f); + EXPECT_NEAR(*iter_y, 0.0f, 1e-3f); + } + } +} + +TEST(PointCloudXyz, RejectsUnsupportedEncoding) +{ + robot_sensor_msgs::Image depth = makeFlatDepthImage(4, 4, 1500); + depth.encoding = robot_sensor_msgs::image_encodings::RGB8; + + const robot_sensor_msgs::CameraInfo info = makeCameraInfo(4, 4); + const robot_sensor_msgs::PointCloud2 cloud = + depth_image_proc::convertDepthToPointCloud(depth, info); + + EXPECT_EQ(cloud.width, 0u); + EXPECT_EQ(cloud.height, 0u); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}