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

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