/********************************************************************* * 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_CONVERSIONS2 #define DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS2 #include <sensor_msgs/Image.h> #include <sensor_msgs/point_cloud2_iterator.h> #include <image_geometry/pinhole_camera_model.h> #include <depth_traits2.h> #include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <limits> namespace depth_image_proc2{ // Handles float or uint16 depths template<typename T> void convert( const sensor_msgs::ImageConstPtr& depth_msg, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, double cx, double cy, double fx, double fy, double range_max = 0.0) { // Use correct principal point from calibration float center_x = cx; float center_y = 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 / fx; float constant_y = unit_scaling / fy; float bad_point = std::numeric_limits<float>::quiet_NaN(); pcl::PointCloud<pcl::PointXYZ>::iterator cloud_iterator = cloud->begin(); const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); int cloud_height = cloud->height; int cloud_width = cloud->width; for (int v = 0; v < cloud_height; ++v, depth_row += row_step) { for (int u = 0; u < cloud_width; ++u, ++cloud_iterator) { 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 { cloud_iterator->x = cloud_iterator->y = cloud_iterator->z = bad_point; continue; } } // Fill in XYZ cloud_iterator->x = (u - center_x) * depth * constant_x; cloud_iterator->y = (v - center_y) * depth * constant_y; cloud_iterator->z = DepthTraits<T>::toMeters(depth); } } } } // namespace depth_image_proc #endif
/********************************************************************* * 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_TRAITS2 #define DEPTH_IMAGE_PROC_DEPTH_TRAITS2 #include <algorithm> #include <limits> namespace depth_image_proc2 { // 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