pcl2numpy.py
Go to the documentation of this file.
00001 from sensor_msgs.msg import PointCloud2
00002 import numpy as np
00003 import struct
00004 import time
00005 
00006 def get_auto(data, wants, flipit=True):
00007     resolution = (data.height, data.width)
00008     img = np.fromstring(data.data, np.float32)
00009     step=img.shape[0]/(data.height*data.width)
00010     ret = []
00011     for w, p in wants:
00012         if w=="pos":
00013             x = img[p::step].reshape(resolution)
00014             y = img[p+1::step].reshape(resolution)
00015             z = img[p+2::step].reshape(resolution) 
00016             if flipit:
00017                 x    = np.flipud(x) 
00018                 y    = np.flipud(y)
00019                 z    = np.flipud(z)
00020             ret.extend([x, y, z])
00021         elif w=="rgb":
00022             rgb = img[p::step]
00023             r = np.zeros(data.height*data.width)
00024             g = np.zeros(data.height*data.width)
00025             b = np.zeros(data.height*data.width)
00026             for i in range(rgb.shape[0]):
00027                 int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00028                 r[i] = (int_rgb & 0xff0000) >> 16
00029                 g[i] = (int_rgb & 0xff00) >> 8
00030                 b[i] = (int_rgb & 0xff)
00031             r = r.reshape(resolution)
00032             g = g.reshape(resolution)
00033             b = b.reshape(resolution)
00034             if flipit:
00035                 r = np.flipud(r)
00036                 g = np.flipud(g)
00037                 b = np.flipud(b)
00038             gray = (0.30*r+0.59*g+0.11*b).astype('uint8')
00039             ret.extend([r,g,b,gray])
00040         elif w=="norm":
00041             n_x = img[p::step].reshape(resolution)
00042             n_y = img[p+1::step].reshape(resolution)
00043             n_z = img[p+2::step].reshape(resolution)
00044             if flipit:
00045                 n_x = np.flipud(n_x)
00046                 n_y = np.flipud(n_y)
00047                 n_z = np.flipud(n_z)
00048             ret.extend([n_x, n_y, n_z])
00049         elif w=="curv":
00050             curv = img[p::step].reshape(resolution)
00051             if flipit:
00052                 curv = np.flipud(curv)
00053             ret.extend([curv])
00054     return ret
00055 
00056 def get_pos_rgb(data):
00057     resolution = (data.height, data.width)
00058     # 3D position for each pixel
00059     img = np.fromstring(data.data, np.float32)
00060 
00061     x = img[0::8].reshape(resolution)
00062     y = img[1::8].reshape(resolution)
00063     z = img[2::8].reshape(resolution) 
00064     rgb = img[4::8]
00065     r = np.zeros(data.height*data.width)
00066     g = np.zeros(data.height*data.width)
00067     b = np.zeros(data.height*data.width)
00068     for i in range(rgb.shape[0]):
00069         int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00070         r[i] = (int_rgb & 0xff0000) >> 16
00071         g[i] = (int_rgb & 0xff00) >> 8
00072         b[i] = (int_rgb & 0xff)
00073     r = r.reshape(resolution)
00074     g = g.reshape(resolution)
00075     b = b.reshape(resolution)
00076     x    = np.flipud(x)
00077     y    = np.flipud(y)
00078     z    = np.flipud(z)
00079     return (x, y, z, r, g, b)
00080 
00081 def get_pos_rgb_gray(data):
00082     resolution = (data.height, data.width)
00083     # 3D position for each pixel
00084     img = np.fromstring(data.data, np.float32)
00085 
00086     x = img[0::8].reshape(resolution)
00087     y = img[1::8].reshape(resolution)
00088     z = img[2::8].reshape(resolution) 
00089     rgb = img[4::8]
00090     r = np.zeros(data.height*data.width)
00091     g = np.zeros(data.height*data.width)
00092     b = np.zeros(data.height*data.width)
00093     for i in range(rgb.shape[0]):
00094         int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00095         r[i] = (int_rgb & 0xff0000) >> 16
00096         g[i] = (int_rgb & 0xff00) >> 8
00097         b[i] = (int_rgb & 0xff)
00098     r = r.reshape(resolution)
00099     g = g.reshape(resolution)
00100     b = b.reshape(resolution)
00101     x    = np.flipud(x)
00102     y    = np.flipud(y)
00103     z    = np.flipud(z)
00104     gray = np.flipud(0.30*r+0.59*g+0.11*b).astype('uint8')
00105     return (x, y, z, r, g, b, gray)
00106 
00107 
00108 def get_pos_rgb_gray_norm(data):
00109     resolution = (data.height, data.width)
00110     # 3D position for each pixel
00111     img = np.fromstring(data.data, np.float32)
00112     
00113     print img.shape
00114     x = img[0::12].reshape(resolution)
00115     y = img[1::12].reshape(resolution)
00116     z = img[2::12].reshape(resolution) 
00117     n_x = img[4::12].reshape(resolution)
00118     n_y = img[5::12].reshape(resolution)
00119     n_z = img[6::12].reshape(resolution)
00120     rgb = img[8::12]
00121     r = np.zeros(data.height*data.width)
00122     g = np.zeros(data.height*data.width)
00123     b = np.zeros(data.height*data.width)
00124     curv = img[9::12].reshape(resolution)
00125     for i in range(rgb.shape[0]):
00126         int_rgb = struct.unpack('i', struct.pack('f', rgb[i]))[0]
00127         r[i] = (int_rgb & 0xff0000) >> 16
00128         g[i] = (int_rgb & 0xff00) >> 8
00129         b[i] = (int_rgb & 0xff)
00130     r = r.reshape(resolution)
00131     g = g.reshape(resolution)
00132     b = b.reshape(resolution)
00133 
00134     x    = np.flipud(x)
00135     y    = np.flipud(y)
00136     z    = np.flipud(z)
00137     n_x  = np.flipud(n_x)
00138     n_y  = np.flipud(n_y)
00139     n_z  = np.flipud(n_z)
00140     curv = np.flipud(curv)
00141     gray = np.flipud(0.30*r+0.59*g+0.11*b).astype('uint8')
00142 
00143     return (x, y, z, n_x, n_y, n_z, r, g, b, curv, gray)


iri_bow_object_detector
Author(s): dmartinez
autogenerated on Fri Dec 6 2013 22:45:46