point_cloud2.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2008, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Author: Jon Binney
34 '''
35 Functions for working with PointCloud2.
36 '''
37 
38 __docformat__ = "restructuredtext en"
39 
40 from .registry import converts_from_numpy, converts_to_numpy
41 
42 import numpy as np
43 from sensor_msgs.msg import PointCloud2, PointField
44 
45 # prefix to the names of dummy fields we add to get byte alignment correct. this needs to not
46 # clash with any actual field names
47 DUMMY_FIELD_PREFIX = '__'
48 
49 # mappings between PointField types and numpy types
50 type_mappings = [(PointField.INT8, np.dtype('int8')), (PointField.UINT8, np.dtype('uint8')), (PointField.INT16, np.dtype('int16')),
51  (PointField.UINT16, np.dtype('uint16')), (PointField.INT32, np.dtype('int32')), (PointField.UINT32, np.dtype('uint32')),
52  (PointField.FLOAT32, np.dtype('float32')), (PointField.FLOAT64, np.dtype('float64'))]
53 pftype_to_nptype = dict(type_mappings)
54 nptype_to_pftype = dict((nptype, pftype) for pftype, nptype in type_mappings)
55 
56 # sizes (in bytes) of PointField types
57 pftype_sizes = {PointField.INT8: 1, PointField.UINT8: 1, PointField.INT16: 2, PointField.UINT16: 2,
58  PointField.INT32: 4, PointField.UINT32: 4, PointField.FLOAT32: 4, PointField.FLOAT64: 8}
59 
60 @converts_to_numpy(PointField, plural=True)
61 def fields_to_dtype(fields, point_step):
62  '''Convert a list of PointFields to a numpy record datatype.
63  '''
64  offset = 0
65  np_dtype_list = []
66  for f in fields:
67  while offset < f.offset:
68  # might be extra padding between fields
69  np_dtype_list.append(('%s%d' % (DUMMY_FIELD_PREFIX, offset), np.uint8))
70  offset += 1
71 
72  dtype = pftype_to_nptype[f.datatype]
73  if f.count != 1:
74  dtype = np.dtype((dtype, f.count))
75 
76  np_dtype_list.append((f.name, dtype))
77  offset += pftype_sizes[f.datatype] * f.count
78 
79  # might be extra padding between points
80  while offset < point_step:
81  np_dtype_list.append(('%s%d' % (DUMMY_FIELD_PREFIX, offset), np.uint8))
82  offset += 1
83 
84  return np_dtype_list
85 
86 
87 @converts_from_numpy(PointField, plural=True)
88 def dtype_to_fields(dtype):
89  '''Convert a numpy record datatype into a list of PointFields.
90  '''
91  fields = []
92  for field_name in dtype.names:
93  np_field_type, field_offset = dtype.fields[field_name]
94  pf = PointField()
95  pf.name = field_name
96  if np_field_type.subdtype:
97  item_dtype, shape = np_field_type.subdtype
98  pf.count = np.prod(shape)
99  np_field_type = item_dtype
100  else:
101  pf.count = 1
102 
103  pf.datatype = nptype_to_pftype[np_field_type]
104  pf.offset = field_offset
105  fields.append(pf)
106  return fields
107 
108 @converts_to_numpy(PointCloud2)
109 def pointcloud2_to_array(cloud_msg, squeeze=True):
110  ''' Converts a rospy PointCloud2 message to a numpy recordarray
111 
112  Reshapes the returned array to have shape (height, width), even if the height is 1.
113 
114  The reason for using np.frombuffer rather than struct.unpack is speed... especially
115  for large point clouds, this will be <much> faster.
116  '''
117  # construct a numpy record type equivalent to the point type of this cloud
118  dtype_list = fields_to_dtype(cloud_msg.fields, cloud_msg.point_step)
119 
120  # parse the cloud into an array
121  cloud_arr = np.frombuffer(cloud_msg.data, dtype_list)
122 
123  # remove the dummy fields that were added
124  cloud_arr = cloud_arr[
125  [fname for fname, _type in dtype_list if not (fname[:len(DUMMY_FIELD_PREFIX)] == DUMMY_FIELD_PREFIX)]]
126 
127  if squeeze and cloud_msg.height == 1:
128  return np.reshape(cloud_arr, (cloud_msg.width,))
129  else:
130  return np.reshape(cloud_arr, (cloud_msg.height, cloud_msg.width))
131 
132 @converts_from_numpy(PointCloud2)
133 def array_to_pointcloud2(cloud_arr, stamp=None, frame_id=None):
134  '''Converts a numpy record array to a sensor_msgs.msg.PointCloud2.
135  '''
136  # make it 2d (even if height will be 1)
137  cloud_arr = np.atleast_2d(cloud_arr)
138 
139  cloud_msg = PointCloud2()
140 
141  if stamp is not None:
142  cloud_msg.header.stamp = stamp
143  if frame_id is not None:
144  cloud_msg.header.frame_id = frame_id
145  cloud_msg.height = cloud_arr.shape[0]
146  cloud_msg.width = cloud_arr.shape[1]
147  cloud_msg.fields = dtype_to_fields(cloud_arr.dtype)
148  cloud_msg.is_bigendian = False # assumption
149  cloud_msg.point_step = cloud_arr.dtype.itemsize
150  cloud_msg.row_step = cloud_msg.point_step*cloud_arr.shape[1]
151  cloud_msg.is_dense = all([np.isfinite(cloud_arr[fname]).all() for fname in cloud_arr.dtype.names])
152  cloud_msg.data = cloud_arr.tostring()
153  return cloud_msg
154 
155 def merge_rgb_fields(cloud_arr):
156  '''Takes an array with named np.uint8 fields 'r', 'g', and 'b', and returns an array in
157  which they have been merged into a single np.float32 'rgb' field. The first byte of this
158  field is the 'r' uint8, the second is the 'g', uint8, and the third is the 'b' uint8.
159 
160  This is the way that pcl likes to handle RGB colors for some reason.
161  '''
162  r = np.asarray(cloud_arr['r'], dtype=np.uint32)
163  g = np.asarray(cloud_arr['g'], dtype=np.uint32)
164  b = np.asarray(cloud_arr['b'], dtype=np.uint32)
165  rgb_arr = np.array((r << 16) | (g << 8) | (b << 0), dtype=np.uint32)
166 
167  # not sure if there is a better way to do this. i'm changing the type of the array
168  # from uint32 to float32, but i don't want any conversion to take place -jdb
169  rgb_arr.dtype = np.float32
170 
171  # create a new array, without r, g, and b, but with rgb float32 field
172  new_dtype = []
173  for field_name in cloud_arr.dtype.names:
174  field_type, field_offset = cloud_arr.dtype.fields[field_name]
175  if field_name not in ('r', 'g', 'b'):
176  new_dtype.append((field_name, field_type))
177  new_dtype.append(('rgb', np.float32))
178  new_cloud_arr = np.zeros(cloud_arr.shape, new_dtype)
179 
180  # fill in the new array
181  for field_name in new_cloud_arr.dtype.names:
182  if field_name == 'rgb':
183  new_cloud_arr[field_name] = rgb_arr
184  else:
185  new_cloud_arr[field_name] = cloud_arr[field_name]
186 
187  return new_cloud_arr
188 
189 def split_rgb_field(cloud_arr):
190  '''Takes an array with a named 'rgb' float32 field, and returns an array in which
191  this has been split into 3 uint 8 fields: 'r', 'g', and 'b'.
192 
193  (pcl stores rgb in packed 32 bit floats)
194  '''
195  rgb_arr = cloud_arr['rgb'].copy()
196  rgb_arr.dtype = np.uint32
197  r = np.asarray((rgb_arr >> 16) & 255, dtype=np.uint8)
198  g = np.asarray((rgb_arr >> 8) & 255, dtype=np.uint8)
199  b = np.asarray(rgb_arr & 255, dtype=np.uint8)
200 
201  # create a new array, without rgb, but with r, g, and b fields
202  new_dtype = []
203  for field_name in cloud_arr.dtype.names:
204  field_type, field_offset = cloud_arr.dtype.fields[field_name]
205  if not field_name == 'rgb':
206  new_dtype.append((field_name, field_type))
207  new_dtype.append(('r', np.uint8))
208  new_dtype.append(('g', np.uint8))
209  new_dtype.append(('b', np.uint8))
210  new_cloud_arr = np.zeros(cloud_arr.shape, new_dtype)
211 
212  # fill in the new array
213  for field_name in new_cloud_arr.dtype.names:
214  if field_name == 'r':
215  new_cloud_arr[field_name] = r
216  elif field_name == 'g':
217  new_cloud_arr[field_name] = g
218  elif field_name == 'b':
219  new_cloud_arr[field_name] = b
220  else:
221  new_cloud_arr[field_name] = cloud_arr[field_name]
222  return new_cloud_arr
223 
224 def get_xyz_points(cloud_array, remove_nans=True, dtype=np.float):
225  '''Pulls out x, y, and z columns from the cloud recordarray, and returns
226  a 3xN matrix.
227  '''
228  # remove crap points
229  if remove_nans:
230  mask = np.isfinite(cloud_array['x']) & np.isfinite(cloud_array['y']) & np.isfinite(cloud_array['z'])
231  cloud_array = cloud_array[mask]
232 
233  # pull out x, y, and z values
234  points = np.zeros(cloud_array.shape + (3,), dtype=dtype)
235  points[...,0] = cloud_array['x']
236  points[...,1] = cloud_array['y']
237  points[...,2] = cloud_array['z']
238 
239  return points
240 
241 def pointcloud2_to_xyz_array(cloud_msg, remove_nans=True):
242  return get_xyz_points(pointcloud2_to_array(cloud_msg), remove_nans=remove_nans)
def converts_from_numpy(msgtype, plural=False)
Definition: registry.py:17
def split_rgb_field(cloud_arr)
def converts_to_numpy(msgtype, plural=False)
Definition: registry.py:9
def pointcloud2_to_array(cloud_msg, squeeze=True)
def dtype_to_fields(dtype)
Definition: point_cloud2.py:88
def pointcloud2_to_xyz_array(cloud_msg, remove_nans=True)
def fields_to_dtype(fields, point_step)
Definition: point_cloud2.py:61
def array_to_pointcloud2(cloud_arr, stamp=None, frame_id=None)
def get_xyz_points(cloud_array, remove_nans=True, dtype=np.float)
def merge_rgb_fields(cloud_arr)


ros_numpy
Author(s): Eric Wieser
autogenerated on Tue Aug 6 2019 03:32:07