~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to rosutils_mrol/rosutils_mrol.py

  • Committer: Vikas Dhiman
  • Date: 2012-05-22 15:26:38 UTC
  • Revision ID: wecacuee@gmail.com-20120522152638-j6srlkgrpdq4kdfe
Added support for userdata in terms of colored voxels. Added benchmark test and corresponding data

Show diffs side-by-side

added added

removed removed

Lines of Context:
29
29
    if len(xyzs) != 0:
30
30
        if type(xyzs[0][0]) == np.float64:
31
31
            xyzs = np.array(xyzs,dtype=np.float32)
32
 
        assert (xyzs.shape[1] != 3) or (xyzs.shape[1] != 6)
 
32
        assert (xyzs.shape[1] != 3) or (xyzs.shape[1] != 6),\
 
33
                "Shape: {0}".format(xyzs.shape)
33
34
        if xyzs.shape[1] == 3:
34
35
            numfields = 3
35
36
        else:
72
73
            data_buf = xyzs.flatten().tostring()
73
74
        else:
74
75
            #import pydb;pydb.set_trace()
75
 
            rgb = np.hstack([np.uint8(xyzs[:,6:2:-1]),np.zeros([xyzs.shape[0],1],dtype=np.uint8)]).copy()
 
76
            rgb = np.hstack([np.uint8(xyzs[:,6:2:-1]),
 
77
                             np.zeros([xyzs.shape[0],1],dtype=np.uint8)]
 
78
                           ).copy()
76
79
            rgb = rgb.view(dtype=float32LE)
77
80
            data = np.hstack([xyzs[:,0:3],rgb])
78
81
            data_buf = data.flatten().tostring()
160
163
    arr /= 1000.0
161
164
    assert(len(arr) > 0)
162
165
    return depth_image.image_to_points(arr, **kwargs)
163