~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to mrol_mapping/depth_image.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:
15
15
    di /= blender_map
16
16
    return di
17
17
 
18
 
def image_to_points(di, max_range=5., fov=np.radians(75)):
 
18
def image_to_points(di, rgb=None, max_range=5., fov=np.radians(75),
 
19
                    timestamp=0):
19
20
    ''' Convert a depth image to point cloud'''
20
 
    
21
 
    # horizontal field of view setting in blender in degrees
22
 
    #fov = np.radians(75)
23
 
 
24
21
    # at edge tan fov/2 = 320 / a where a is the adjacent pixel length
25
22
    a = 0.5 * di.shape[1] / np.tan(fov/2)
26
 
 
27
 
    cols = np.arange(0, di.shape[1]) - di.shape[1]/2 + 0.5
28
 
    rows = np.arange(0, di.shape[0]) - di.shape[0]/2 + 0.5
 
23
    ci = np.arange(0, di.shape[1])
 
24
    ri = np.arange(0, di.shape[0])
 
25
    cols = ci - di.shape[1]/2 + 0.5
 
26
    rows = ri - di.shape[0]/2 + 0.5
29
27
    rows.shape = -1, 1
30
28
 
31
29
    f = di / a
50
48
    
51
49
    xyzs = np.vstack((X[inds], Y[inds], Z[inds])).T
52
50
    assert(len(xyzs))
53
 
    pc = pointcloud.PointCloud(xyzs)
 
51
    pc = pointcloud.PointCloud(xyzs, timestamp=timestamp)
 
52
    if rgb is not None:
 
53
        rgbfiltered = rgb[inds]
 
54
        pc.userdata = rgbfiltered
54
55
    return pc
55
56
    
56
57
def depth_discont_simple(di, tol=0.2):