예제 #1
0
def trace_path(fcs, path_coords, mesh, save_ims = True):
    coords_proj = mathu.project_points_to_plane(fcs.origin,
                                                fcs.a3,
                                                path_coords)
    coords_proj_offset_world = numpy.subtract(coords_proj, fcs.origin)
    coords_proj_offset_floor = fcs.convert_points_from_world_frame(coords_proj_offset_world)

    last_cpof = None
    path_cnt = 0
    for cpof in coords_proj_offset_floor:
        if last_cpof is not None:
            offset = cpof - last_cpof
        else:
            offset = cpof
        slide_mesh_along_floor(mesh, fcs, offset[0], offset[1])
        last_cpof = cpof
        if save_ims:
            mayavi.mlab.savefig("path_{:08d}.png".format(path_cnt))
        path_cnt += 1
        yield
예제 #2
0
def trace_path(fcs, path_coords, mesh, save_ims=True):
    coords_proj = mathu.project_points_to_plane(fcs.origin, fcs.a3,
                                                path_coords)
    coords_proj_offset_world = numpy.subtract(coords_proj, fcs.origin)
    coords_proj_offset_floor = fcs.convert_points_from_world_frame(
        coords_proj_offset_world)

    last_cpof = None
    path_cnt = 0
    for cpof in coords_proj_offset_floor:
        if last_cpof is not None:
            offset = cpof - last_cpof
        else:
            offset = cpof
        slide_mesh_along_floor(mesh, fcs, offset[0], offset[1])
        last_cpof = cpof
        if save_ims:
            mayavi.mlab.savefig("path_{:08d}.png".format(path_cnt))
        path_cnt += 1
        yield
예제 #3
0
def calc_scene_floor_from_gravity_and_points(normal, data, lowest_n = 500):
    dists = numpy.dot(data[:, :3], -normal)


    lowest_point_inds = numpy.argsort(dists)[::-1]
    
    median_lowest_point = data[lowest_point_inds[250], :3]
    # mayavi.mlab.points3d(*median_lowest_point, scale_factor = .1)


    print "projecting points to plane"
    proj_points = mathu.project_points_to_plane(median_lowest_point,
                                                normal,
                                                data[:, :3])

    #calcuate floor extent and coordinate system
    floor_center = proj_points.mean(axis = 0)
    floor_height = numpy.dot(floor_center, normal / numpy.linalg.norm(normal))

    floor_vecs = proj_points - floor_center
    floor_dists = numpy.linalg.norm(floor_vecs, axis = 1)
    furthest_floor_vec = floor_vecs[numpy.argmax(floor_dists), :]

    bvec1 = furthest_floor_vec
    bvec2 = numpy.cross(normal, furthest_floor_vec)

    v1dists = numpy.dot(floor_vecs, bvec1)
    v1as = numpy.argsort(v1dists)
    
    v2dists = numpy.dot(floor_vecs, bvec2)
    v2as = numpy.argsort(v2dists)

    v1min, v1max = floor_vecs[v1as[0], :] + floor_center, floor_vecs[v1as[-1], :] + floor_center
    v2min, v2max = floor_vecs[v2as[0], :] + floor_center, floor_vecs[v2as[-1], :] + floor_center

    ff2 = floor_vecs[v2as[-1], :]

    # set up coordinate system
    tds_floor = mathu.three_d_coord_system(floor_center, bvec1, bvec2, normal)
    # return tds_floor, v1min, v1max, v2min, v2max
    return tds_floor
예제 #4
0
def calc_scene_floor_from_gravity_and_points(normal, data, lowest_n=500):
    dists = numpy.dot(data[:, :3], -normal)

    lowest_point_inds = numpy.argsort(dists)[::-1]

    median_lowest_point = data[lowest_point_inds[250], :3]
    # mayavi.mlab.points3d(*median_lowest_point, scale_factor = .1)

    print "projecting points to plane"
    proj_points = mathu.project_points_to_plane(median_lowest_point, normal,
                                                data[:, :3])

    #calcuate floor extent and coordinate system
    floor_center = proj_points.mean(axis=0)
    floor_height = numpy.dot(floor_center, normal / numpy.linalg.norm(normal))

    floor_vecs = proj_points - floor_center
    floor_dists = numpy.linalg.norm(floor_vecs, axis=1)
    furthest_floor_vec = floor_vecs[numpy.argmax(floor_dists), :]

    bvec1 = furthest_floor_vec
    bvec2 = numpy.cross(normal, furthest_floor_vec)

    v1dists = numpy.dot(floor_vecs, bvec1)
    v1as = numpy.argsort(v1dists)

    v2dists = numpy.dot(floor_vecs, bvec2)
    v2as = numpy.argsort(v2dists)

    v1min, v1max = floor_vecs[v1as[0], :] + floor_center, floor_vecs[
        v1as[-1], :] + floor_center
    v2min, v2max = floor_vecs[v2as[0], :] + floor_center, floor_vecs[
        v2as[-1], :] + floor_center

    ff2 = floor_vecs[v2as[-1], :]

    # set up coordinate system
    tds_floor = mathu.three_d_coord_system(floor_center, bvec1, bvec2, normal)
    # return tds_floor, v1min, v1max, v2min, v2max
    return tds_floor