DuongTD/depth_image_proc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2026-06-24 17:42:12 +07:00
commit 539b692d42
18 changed files with 2170 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

295
CMakeLists.txt Normal file
View File

@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(${PROJECT_NAME}
PUBLIC
${catkin_LIBRARIES}
PRIVATE
Boost::thread
Eigen3::Eigen
${TF3_LIBRARY}
)
# ========================================================
# Standalone linking
# ========================================================
else()
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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()

102
include/robot_depth_image_proc/depth_conversions.h Normal file
View File

@@ -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 <robot_sensor_msgs/Image.h>
#include <robot_sensor_msgs/CameraInfo.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <robot_image_geometry/pinhole_camera_model.h>
#include <robot_depth_image_proc/depth_traits.h>
#include <limits>
namespace depth_image_proc {
typedef robot_sensor_msgs::PointCloud2 PointCloud;
// Handles float or uint16 depths
template<typename T>
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<T>::toMeters( T(1) );
float constant_x = unit_scaling / model.fx();
float constant_y = unit_scaling / model.fy();
float bad_point = std::numeric_limits<float>::quiet_NaN();
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
const T* depth_row = reinterpret_cast<const T*>(&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<T>::valid(depth))
{
if (range_max != 0.0)
{
depth = DepthTraits<T>::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<T>::toMeters(depth);
}
}
}
} // namespace depth_image_proc
#endif

74
include/robot_depth_image_proc/depth_traits.h Normal file
View File

@@ -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 <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <vector>
namespace depth_image_proc {
// Encapsulate differences between processing float and uint16_t depths
template<typename T> struct DepthTraits {};
template<>
struct DepthTraits<uint16_t>
{
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<uint8_t>& buffer) {} // Do nothing - already zero-filled
};
template<>
struct DepthTraits<float>
{
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<uint8_t>& buffer)
{
float* start = reinterpret_cast<float*>(&buffer[0]);
float* end = reinterpret_cast<float*>(&buffer[0] + buffer.size());
std::fill(start, end, std::numeric_limits<float>::quiet_NaN());
}
};
} // namespace depth_image_proc
#endif

17
include/robot_depth_image_proc/point_cloud_xyz.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef ROBOT_DEPTH_IMAGE_PROC_POINT_CLOUD_XYZ_H
#define ROBOT_DEPTH_IMAGE_PROC_POINT_CLOUD_XYZ_H
#include <robot_sensor_msgs/CameraInfo.h>
#include <robot_sensor_msgs/Image.h>
#include <robot_sensor_msgs/PointCloud2.h>
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

132
include/robot_depth_image_proc/ros_message_conversions.h Normal file
View File

@@ -0,0 +1,132 @@
#ifndef ROBOT_DEPTH_IMAGE_PROC_ROS_MESSAGE_CONVERSIONS_H
#define ROBOT_DEPTH_IMAGE_PROC_ROS_MESSAGE_CONVERSIONS_H
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointField.h>
#include <std_msgs/Header.h>
#include <robot/time.h>
#include <robot_sensor_msgs/CameraInfo.h>
#include <robot_sensor_msgs/Image.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_std_msgs/Header.h>
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

27
launch/depth_image_proc_realsense.launch Normal file
View File

@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<launch>
<!-- Topics from RealSense depth_publisher.py -->
<arg name="depth_topic" default="/camera/depth/image_raw"/>
<arg name="camera_info_topic" default="/camera/depth/camera_info"/>
<arg name="cloud_topic" default="/camera/depth/points"/>
<arg name="fixed_frame" default="map"/>
<arg name="publish_tf" default="true"/>
<arg name="rviz" default="true"/>
<node pkg="robot_depth_image_proc"
type="depth_image_proc_node"
name="depth_image_proc"
output="screen">
<param name="depth_topic" value="$(arg depth_topic)"/>
<param name="camera_info_topic" value="$(arg camera_info_topic)"/>
<param name="cloud_topic" value="$(arg cloud_topic)"/>
<param name="fixed_frame" value="$(arg fixed_frame)"/>
<param name="publish_tf" value="$(arg publish_tf)"/>
</node>
<node if="$(arg rviz)"
pkg="rviz"
type="rviz"
name="rviz"
args="-d $(find robot_depth_image_proc)/rviz/depth_image_proc_realsense.rviz"/>
</launch>

31
launch/depth_image_proc_test.launch Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<launch>
<arg name="frame_id" default="camera_depth_optical_frame"/>
<arg name="fixed_frame" default="map"/>
<arg name="publish_rate" default="10.0"/>
<arg name="width" default="640"/>
<arg name="height" default="480"/>
<arg name="rviz" default="true"/>
<node pkg="robot_depth_image_proc"
type="depth_image_proc_test_node"
name="depth_image_proc_test"
output="screen">
<param name="frame_id" value="$(arg frame_id)"/>
<param name="fixed_frame" value="$(arg fixed_frame)"/>
<param name="publish_rate" value="$(arg publish_rate)"/>
<param name="width" value="$(arg width)"/>
<param name="height" value="$(arg height)"/>
</node>
<node pkg="tf2_ros"
type="static_transform_publisher"
name="camera_depth_tf"
args="0 0 0 0 0 0 $(arg fixed_frame) $(arg frame_id)"/>
<node if="$(arg rviz)"
pkg="rviz"
type="rviz"
name="rviz"
args="-d $(find robot_depth_image_proc)/rviz/depth_image_proc_test.rviz"/>
</launch>

53
package.xml Normal file
View File

@@ -0,0 +1,53 @@
<package>
<name>robot_depth_image_proc</name>
<version>0.7.10</version>
<description>
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.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/robot_depth_image_proc</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<build_depend>boost</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>robot_image_geometry</build_depend>
<build_depend>eigen</build_depend>
<build_depend>robot_sensor_msgs</build_depend>
<build_depend>robot_cpp</build_depend>
<build_depend>tf3</build_depend>
<build_depend>data_convert</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf2_ros</build_depend>
<run_depend>boost</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>robot_image_geometry</run_depend>
<run_depend>eigen</run_depend>
<run_depend>robot_sensor_msgs</run_depend>
<run_depend>robot_cpp</run_depend>
<run_depend>tf3</run_depend>
<run_depend>data_convert</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>rviz</run_depend>
</package>

67
rviz/depth_image_proc_realsense.rviz Normal file
View File

@@ -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

67
rviz/depth_image_proc_test.rviz Normal file
View File

@@ -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

140
src/convert_metric.cpp Normal file
View File

@@ -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 <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <boost/thread.hpp>
namespace depth_image_proc {
namespace enc = sensor_msgs::image_encodings;
class ConvertMetricNodelet : public nodelet::Nodelet
{
// Subscriptions
boost::shared_ptr<image_transport::ImageTransport> 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<boost::mutex> 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<boost::mutex> 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<float>::quiet_NaN ();
const uint16_t* raw_data = reinterpret_cast<const uint16_t*>(&raw_msg->data[0]);
float* depth_data = reinterpret_cast<float*>(&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<const float*>(&raw_msg->data[0]);
uint16_t* depth_data = reinterpret_cast<uint16_t*>(&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/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(depth_image_proc::ConvertMetricNodelet,nodelet::Nodelet);

142
src/crop_foremost.cpp Executable file
View File

@@ -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 <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <boost/thread.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
namespace depth_image_proc {
namespace enc = sensor_msgs::image_encodings;
class CropForemostNodelet : public nodelet::Nodelet
{
// Subscriptions
boost::shared_ptr<image_transport::ImageTransport> 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<boost::mutex> 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<boost::mutex> 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/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(depth_image_proc::CropForemostNodelet,nodelet::Nodelet);

148
src/depth_image_proc_node.cpp Normal file
View File

@@ -0,0 +1,148 @@
#include <mutex>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <robot_depth_image_proc/point_cloud_xyz.h>
#include <robot_depth_image_proc/ros_message_conversions.h>
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<sensor_msgs::PointCloud2>(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<std::mutex> 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<std::mutex> 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;
}

211
src/depth_image_proc_test_node.cpp Normal file
View File

@@ -0,0 +1,211 @@
#include <cmath>
#include <cstdint>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <robot_depth_image_proc/point_cloud_xyz.h>
#include <robot_depth_image_proc/ros_message_conversions.h>
#include <robot_sensor_msgs/image_encodings.h>
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<size_t>(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<uint16_t*>(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<double>(u) - kCx,
static_cast<double>(v) - kCy);
const double ripple_m = 0.05 * std::sin(radial * 0.05);
const uint16_t depth_mm = static_cast<uint16_t>((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<sensor_msgs::PointCloud2>("depth/points", 1);
depth_pub_ = nh.advertise<sensor_msgs::Image>("depth/image", 1);
camera_info_pub_ = nh.advertise<sensor_msgs::CameraInfo>("depth/camera_info", 1, true);
camera_info_ = makeCameraInfo(
static_cast<uint32_t>(width_),
static_cast<uint32_t>(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<uint32_t>(width_),
static_cast<uint32_t>(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;
}

189
src/disparity.cpp Normal file
View File

@@ -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 <boost/version.hpp>
#if ((BOOST_VERSION / 100) % 1000) >= 53
#include <boost/thread/lock_guard.hpp>
#endif
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/image_encodings.h>
#include <stereo_msgs/DisparityImage.h>
#include <depth_image_proc/depth_traits.h>
namespace depth_image_proc {
namespace enc = sensor_msgs::image_encodings;
class DisparityNodelet : public nodelet::Nodelet
{
boost::shared_ptr<image_transport::ImageTransport> left_it_;
ros::NodeHandlePtr right_nh_;
image_transport::SubscriberFilter sub_depth_image_;
message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
typedef message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> Sync;
boost::shared_ptr<Sync> 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<typename T>
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<double>::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<boost::mutex> lock(connect_mutex_);
pub_disparity_ = left_nh.advertise<stereo_msgs::DisparityImage>("disparity", 1, connect_cb, connect_cb);
}
// Handles (un)subscribing when clients (un)subscribe
void DisparityNodelet::connectCb()
{
boost::lock_guard<boost::mutex> 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<uint16_t>(depth_msg, disp_msg);
}
else if (depth_msg->encoding == enc::TYPE_32FC1)
{
convert<float>(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<typename T>
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<T>::toMeters( T(1) );
float constant = disp_msg->f * disp_msg->T / unit_scaling;
const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
int row_step = depth_msg->step / sizeof(T);
float* disp_data = reinterpret_cast<float*>(&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<T>::valid(depth))
*disp_data = constant / depth;
++disp_data;
}
depth_row += row_step;
}
}
} // namespace depth_image_proc
// Register as nodelet
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(depth_image_proc::DisparityNodelet,nodelet::Nodelet);

48
src/point_cloud_xyz.cpp Normal file
View File

@@ -0,0 +1,48 @@
#include <robot/robot.h>
#include <robot_depth_image_proc/depth_conversions.h>
#include <robot_depth_image_proc/point_cloud_xyz.h>
#include <robot_image_geometry/pinhole_camera_model.h>
#include <robot_sensor_msgs/image_encodings.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
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<uint16_t>(depth_msg, cloud_msg, model);
}
else if (depth_msg.encoding == enc::TYPE_32FC1)
{
convert<float>(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

313
src/register.cpp Normal file
View File

@@ -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 <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <depth_image_proc/depth_traits.h>
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<image_transport::ImageTransport> it_depth_;
// Subscriptions
image_transport::SubscriberFilter sub_depth_image_;
message_filters::Subscriber<sensor_msgs::CameraInfo> sub_depth_info_, sub_rgb_info_;
boost::shared_ptr<tf2_ros::Buffer> tf_buffer_;
boost::shared_ptr<tf2_ros::TransformListener> tf_;
typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> SyncPolicy;
typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
boost::shared_ptr<Synchronizer> 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<typename T>
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<boost::mutex> 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<boost::mutex> 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<uint16_t>(depth_image_msg, registered_msg, depth_to_rgb);
}
else if (depth_image_msg->encoding == enc::TYPE_32FC1)
{
convert<float>(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<typename T>
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<T>::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<const T*>(&depth_msg->data[0]);
int row_step = depth_msg->step / sizeof(T);
T* registered_data = reinterpret_cast<T*>(&registered_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<T>::valid(raw_depth))
continue;
double depth = DepthTraits<T>::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<T>::fromMeters(xyz_rgb.z());
// Validity and Z-buffer checks
if (!DepthTraits<T>::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<T>::fromMeters(0.5*(xyz_rgb_1.z()+xyz_rgb_2.z()));
// Validity and Z-buffer checks
if (!DepthTraits<T>::valid(reg_depth) || reg_depth > new_depth)
reg_depth = new_depth;
}
}
}
}
}
}
} // namespace depth_image_proc
// Register as nodelet
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet,nodelet::Nodelet);

114
test/test_point_cloud_xyz.cpp Normal file
View File

@@ -0,0 +1,114 @@
#include <gtest/gtest.h>
#include <cmath>
#include <cstdint>
#include <robot_depth_image_proc/point_cloud_xyz.h>
#include <robot_sensor_msgs/image_encodings.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
namespace
{
robot_sensor_msgs::CameraInfo makeCameraInfo(uint32_t width, uint32_t height)
{
const double cx = (static_cast<double>(width) - 1.0) * 0.5;
const double cy = (static_cast<double>(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<size_t>(height) * image.step);
auto* depth = reinterpret_cast<uint16_t*>(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<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
const float expected_z = static_cast<float>(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();
}