depth_conversions.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #ifndef DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS
35 #define DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS
36 
37 #include <sensor_msgs/Image.h>
41 
42 #include <limits>
43 
44 namespace depth_image_proc {
45 
46 typedef sensor_msgs::PointCloud2 PointCloud;
47 
48 // Handles float or uint16 depths
49 template<typename T>
50 void convert(
51  const sensor_msgs::ImageConstPtr& depth_msg,
52  PointCloud::Ptr& cloud_msg,
54  double range_max = 0.0)
55 {
56  // Use correct principal point from calibration
57  float center_x = model.cx();
58  float center_y = model.cy();
59 
60  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
61  double unit_scaling = DepthTraits<T>::toMeters( T(1) );
62  float constant_x = unit_scaling / model.fx();
63  float constant_y = unit_scaling / model.fy();
64  float bad_point = std::numeric_limits<float>::quiet_NaN();
65 
66  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
67  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
68  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
69  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
70  int row_step = depth_msg->step / sizeof(T);
71  for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
72  {
73  for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
74  {
75  T depth = depth_row[u];
76 
77  // Missing points denoted by NaNs
78  if (!DepthTraits<T>::valid(depth))
79  {
80  if (range_max != 0.0)
81  {
82  depth = DepthTraits<T>::fromMeters(range_max);
83  }
84  else
85  {
86  *iter_x = *iter_y = *iter_z = bad_point;
87  continue;
88  }
89  }
90 
91  // Fill in XYZ
92  *iter_x = (u - center_x) * depth * constant_x;
93  *iter_y = (v - center_y) * depth * constant_y;
94  *iter_z = DepthTraits<T>::toMeters(depth);
95  }
96  }
97 }
98 
99 } // namespace depth_image_proc
100 
101 #endif
void convert(const sensor_msgs::ImageConstPtr &depth_msg, PointCloud::Ptr &cloud_msg, const image_geometry::PinholeCameraModel &model, double range_max=0.0)
sensor_msgs::PointCloud2 PointCloud


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed Dec 7 2022 03:25:21