Python Vector.xyzw方法代码示例

# 需要导入模块: from mathutils import Vector [as 别名]
# 或者: from mathutils.Vector import xyzw [as 别名]
    def resize_primary_brush(self, radius):
        context = bpy.context
        primary_brush = self.primary_brush
        region_height = context.region.height
        region_width = context.region.width

        # Determine the world space radius of the primary brush necessary to
        # project a circle onto the view plane with the specified region space
        # radius.

        # Determine the z-depth of the primary brush's center in normalized
        # device coordinates.
        projection_matrix = context.region_data.perspective_matrix
        co = primary_brush.center.copy()
        co.resize(4)
        co.w = 1
        co.xyzw = projection_matrix * co
        w = co.w
        co.xyz /= w
        NDC_z_depth = co.z

        # Determine the region space coordinates of the primary brush's center.
        region_x = (co.x + 1) * region_width / 2
        region_y = (co.y + 1) * region_height / 2

        # Determine the NDC coordinates of a point on the edge of the
        # circle that should result from projecting the brush onto the view
        # plane.
        co = Vector((region_x, region_y)) + Vector((radius, 0))

        co.x = co.x * 2 / region_width - 1
        co.y = co.y * 2 / region_height - 1
        co.resize(3)
        co.z = NDC_z_depth

        # Calculate the world space radius of the primary brush.
        co.resize(4)
        co.w = 1
        co.xyzw = projection_matrix.inverted() * co
        w = co.w
        co.resize(3)
        co.xyz /= w
        primary_brush.radius = (co - primary_brush.center).length

# 需要导入模块: from mathutils import Vector [as 别名]
# 或者: from mathutils.Vector import xyzw [as 别名]
    def discard_outside_of_view(self, view): 
        # Assume that the mesh object's bounding box is fully contained in the
        # view projection, and test this assumption.
        bounding_box = mesh_object.bound_box
        bounding_box_contained_in_projection = True

        # Transform the coordinates of each vertex of the bounding box from
        # object space to clip space.  As soon as any single vertex is
        # found to be outside of the view projection, duly indicate, and exit
        # the loop.
        projection_matrix = view.projection_matrix
        for vertex in bounding_box:
            co = Vector(vertex)
            co.resize(4)
            co.w = 1
            co.xyzw = projection_matrix * co
            w = co.w

            # Determine if the coordinates are within the view projection.
            if abs(co.x) > w or\
               abs(co.y) > w or\
               abs(co.z) > w:
                bounding_box_contained_in_projection = False
                break

        # If the bounding box is not entirely contained within the view
        # projection then some vertices may exist outside of the view
        # projection.
        if not bounding_box_contained_in_projection: 
            clip_space_map = self.coordinate_map

            # Retain each clip space vertex that is inside of the view
            # projection.
            indices = self.indices
            self.indices = [
                index
                for index in indices
                if abs(clip_space_map[index].x) < clip_space_map[index].w and\
                   abs(clip_space_map[index].y) < clip_space_map[index].w and\
                   abs(clip_space_map[index].z) < clip_space_map[index].w
            ] 

# 需要导入模块: from mathutils import Vector [as 别名]
# 或者: from mathutils.Vector import xyzw [as 别名]
def scan_advanced(scanner_object, max_distance = 10.0, evd_file=None, add_blender_mesh = False, 
                  add_noisy_blender_mesh = False, tof_res_x = 176, tof_res_y = 144, 
                  lens_angle_w=43.6, lens_angle_h=34.6, flength = 10.0,  evd_last_scan=True, 
                  noise_mu=0.0, noise_sigma=0.004, timestamp = 0.0, backfolding=False,
                  world_transformation=Matrix()):

    inv_scan_x = scanner_object.inv_scan_x
    inv_scan_y = scanner_object.inv_scan_y
    inv_scan_z = scanner_object.inv_scan_z  

    start_time = time.time()


    #10.0mm is currently the distance between the focal point and the sensor
    sensor_width = 2 * math.tan(deg2rad(lens_angle_w/2.0)) * 10.0
    sensor_height = 2 * math.tan(deg2rad(lens_angle_h/2.0)) * 10.0

    if tof_res_x == 0 or tof_res_y == 0:
        raise ValueError("Resolution must be > 0")

    pixel_width = sensor_width / float(tof_res_x)
    pixel_height = sensor_height / float(tof_res_y)


    bpy.context.scene.render.resolution_percentage

    width = bpy.context.scene.render.resolution_x
    height = bpy.context.scene.render.resolution_y
    cx = float(tof_res_x) /2.0
    cy = float(tof_res_y) /2.0 




    evd_buffer = []
    rays = []
    ray_info = []

    ray = Vector([0.0,0.0,0.0])
    for x in range(tof_res_x):
        for y in range(tof_res_y):
            """Calculate a vector that originates at the principal point
               and points to the pixel in the sensor. This vector is then
               scaled to the maximum scanning distance 
            """ 
            physical_x = float(x-cx) * pixel_width
            physical_y = float(y-cy) * pixel_height
            physical_z = -float(flength)
            ray.xyz = [physical_x, physical_y, physical_z]
            ray.normalize()
            final_ray = max_distance*ray
            rays.extend([final_ray[0],final_ray[1],final_ray[2]])


            """ pitch and yaw are added for completeness, normally they are
                not provided by a ToF Camera but can be derived 
                from the pixel position and the camera parameters.
            """
            yaw = math.atan(physical_x/flength)
            pitch = math.atan(physical_y/flength)

            ray_info.append([yaw, pitch, timestamp])
            

    returns = blensor.scan_interface.scan_rays(rays, max_distance, inv_scan_x = inv_scan_x, inv_scan_y = inv_scan_y, inv_scan_z = inv_scan_z)

    verts = []
    verts_noise = []
    evd_storage = evd.evd_file(evd_file, tof_res_x, tof_res_y, max_distance)

    reusable_vector = Vector([0.0,0.0,0.0,0.0])
    for i in range(len(returns)):
        idx = returns[i][-1]
        distance_noise =  random.gauss(noise_mu, noise_sigma)
        #If everything works substitute the previous line with this
        #distance_noise =  pixel_noise[returns[idx][-1]] + random.gauss(noise_mu, noise_sigma) 

        reusable_vector.xyzw = [returns[i][1],returns[i][2],returns[i][3],1.0]
        vt = (world_transformation * reusable_vector).xyz
        v = [returns[i][1],returns[i][2],returns[i][3]]
        verts.append ( vt )
        vector_length = math.sqrt(v[0]**2+v[1]**2+v[2]**2)
        norm_vector = [v[0]/vector_length, v[1]/vector_length, v[2]/vector_length]


        vector_length_noise = vector_length+distance_noise
        if backfolding:
           #Distances > max_distance/2..max_distance are mapped to 0..max_distance/2
           if vector_length_noise >= max_distance/2.0:
               vector_length_noise = vector_length_noise - max_distance/2.0

        reusable_vector.xyzw = [norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
        v_noise = (world_transformation * reusable_vector).xyz
        verts_noise.append( v_noise )

        evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5], idx=returns[i][-1])

    if evd_file:
        evd_storage.appendEvdFile()

#.........这里部分代码省略.........

# 需要导入模块: from mathutils import Vector [as 别名]
# 或者: from mathutils.Vector import xyzw [as 别名]
def scan_advanced(scanner_object, rotation_speed = 25.0, simulation_fps=24, angle_resolution = 0.5, max_distance = 90, evd_file=None,noise_mu=0.0, noise_sigma=0.03, start_angle = -35, end_angle = 50, evd_last_scan=True, add_blender_mesh = False, add_noisy_blender_mesh = False, simulation_time = 0.0,laser_mirror_distance=0.05, world_transformation=Matrix()):
    inv_scan_x = scanner_object.inv_scan_x
    inv_scan_y = scanner_object.inv_scan_y
    inv_scan_z = scanner_object.inv_scan_z   

    start_time = time.time()

    current_time = simulation_time
    delta_rot = angle_resolution*math.pi/180

    evd_storage = evd.evd_file(evd_file)

    xaxis = Vector([1,0,0])
    yaxis = Vector([0,1,0])
    zaxis = Vector([0,0,1])

    rays = []
    ray_info = []

    angles = end_angle-start_angle
    steps_per_rotation = angles/angle_resolution
    time_per_step = (1.0/rotation_speed) / steps_per_rotation

    lines = (end_angle-start_angle)/angle_resolution

    for line in range(int(lines)):
        for laser_idx in range(len(laser_angles)):
            current_angle = start_angle + float(line)*angles/float(lines)
            [ray, origion, laser_angle] = calculateRay(laser_angles[laser_idx], deg2rad(current_angle), laser_mirror_distance) 
            #TODO: Use the origin to cast the ray. Requires changes to the blender patch
            rot_angle = 1e-6 + current_angle + 180.0
            timestamp = ( (rot_angle-180.0)/angle_resolution) * time_per_step 
            rot_angle = rot_angle%360.0
            ray_info.append([deg2rad(rot_angle), laser_angle, timestamp])
            
            rays.extend([ray[0],ray[1],ray[2]])


    returns = blensor.scan_interface.scan_rays(rays, max_distance, inv_scan_x = inv_scan_x, inv_scan_y = inv_scan_y, inv_scan_z = inv_scan_z)

    reusable_vector = Vector([0.0,0.0,0.0,0.0])
    for i in range(len(returns)):
        idx = returns[i][-1]
        reusable_vector.xyzw = [returns[i][1],returns[i][2],returns[i][3],1.0]
        vt = (world_transformation * reusable_vector).xyz
        v = [returns[i][1],returns[i][2],returns[i][3]]

        distance_noise =  laser_noise[idx%len(laser_noise)] + random.gauss(noise_mu, noise_sigma) 
        vector_length = math.sqrt(v[0]**2+v[1]**2+v[2]**2)
        norm_vector = [v[0]/vector_length, v[1]/vector_length, v[2]/vector_length]
        vector_length_noise = vector_length+distance_noise
        reusable_vector.xyzw = [norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
        v_noise = (world_transformation * reusable_vector).xyz

        evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])

    current_angle = start_angle+float(float(int(lines))*angle_resolution)
            
    if evd_file:
        evd_storage.appendEvdFile()

    if not evd_storage.isEmpty():
        scan_data = numpy.array(evd_storage.buffer)
        additional_data = None
        if scanner_object.store_data_in_mesh:
            additional_data = evd_storage.buffer

        if add_blender_mesh:
            mesh_utils.add_mesh_from_points_tf(scan_data[:,5:8], "Scan", world_transformation, buffer=additional_data)

        if add_noisy_blender_mesh:
            mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "NoisyScan", world_transformation, buffer=additional_data) 

        bpy.context.scene.update()

    end_time = time.time()
    scan_time = end_time-start_time
    print ("Elapsed time: %.3f"%(scan_time))

    return True, current_angle, scan_time

# 需要导入模块: from mathutils import Vector [as 别名]
# 或者: from mathutils.Vector import xyzw [as 别名]
def scan_advanced(scanner_object, rotation_speed = 10.0, simulation_fps=24, angle_resolution = 0.1728, max_distance = 120, evd_file=None,noise_mu=0.0, noise_sigma=0.03, start_angle = 0.0, end_angle = 360.0, evd_last_scan=True, add_blender_mesh = False, add_noisy_blender_mesh = False, frame_time = (1.0 / 24.0), simulation_time = 0.0, world_transformation=Matrix()):
    start_time = time.time()

    current_time = simulation_time
    delta_rot = angle_resolution*math.pi/180

    evd_storage = evd.evd_file(evd_file)

    xaxis = Vector([1,0,0])
    yaxis = Vector([0,1,0])
    zaxis = Vector([0,0,1])

    rays = []
    ray_info = []

    steps_per_rotation = 360.0/angle_resolution
    time_per_step = (1.0 / rotation_speed) / steps_per_rotation
    angles = end_angle-start_angle
  
    lines = (end_angle-start_angle)/angle_resolution
    ray = Vector([0.0,0.0,0.0])
    for line in range(int(lines)):
        for laser_idx in range(len(laser_angles)):
            ray.xyz = [0,0,max_distance]
            rot_angle = 1e-6 + start_angle+float(line)*angle_resolution + 180.0
            timestamp = ( (rot_angle-180.0)/angle_resolution) * time_per_step 
            rot_angle = rot_angle%360.0
            ray_info.append([deg2rad(rot_angle), deg2rad(laser_angles[laser_idx]), timestamp])
            
            rotator = Euler( [deg2rad(-laser_angles[laser_idx]), deg2rad(rot_angle), 0.0] )
            ray.rotate( rotator )
            rays.extend([ray[0],ray[1],ray[2]])

    returns = blensor.scan_interface.scan_rays(rays, max_distance)
    verts = []
    verts_noise = []

#    for idx in range((len(rays)//3)):
    
    reusable_4dvector = Vector([0.0,0.0,0.0,0.0])
    
    for i in range(len(returns)):
        idx = returns[i][-1]
        reusable_4dvector.xyzw = (returns[i][1],returns[i][2],returns[i][3],1.0)
        vt = (world_transformation * reusable_4dvector).xyz
        v = [returns[i][1],returns[i][2],returns[i][3]]
        verts.append ( vt )

        distance_noise =  laser_noise[idx%len(laser_noise)] + random.gauss(noise_mu, noise_sigma) 
        vector_length = math.sqrt(v[0]**2+v[1]**2+v[2]**2)
        norm_vector = [v[0]/vector_length, v[1]/vector_length, v[2]/vector_length]
        vector_length_noise = vector_length+distance_noise
        reusable_4dvector.xyzw=[norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
        v_noise = (world_transformation * reusable_4dvector).xyz
        verts_noise.append( v_noise )

        evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])


    current_angle = start_angle+float(float(int(lines))*angle_resolution)

    pre_write_time = time.time()
            
    if evd_file:
        evd_storage.appendEvdFile()

    if add_blender_mesh:
        mesh_utils.add_mesh_from_points_tf(verts, "Scan", world_transformation)

    if add_noisy_blender_mesh:
        mesh_utils.add_mesh_from_points_tf(verts_noise, "NoisyScan", world_transformation) 
        
    bpy.context.scene.update()

    end_time = time.time()
    scan_time = pre_write_time-start_time
    total_time = end_time-start_time
    print ("Elapsed time: %.3f (scan: %.3f)"%(total_time, scan_time))

    return True, current_angle, scan_time

# 需要导入模块: from mathutils import Vector [as 别名]
# 或者: from mathutils.Vector import xyzw [as 别名]
def scan_advanced(scanner_object, simulation_fps=24, evd_file=None,noise_mu=0.0, evd_last_scan=True, add_blender_mesh = False, add_noisy_blender_mesh = False, simulation_time = 0.0,laser_mirror_distance=0.05, world_transformation=Matrix()):
    
    
    angle_resolution=scanner_object.generic_angle_resolution
    max_distance=scanner_object.generic_max_dist
    start_angle=scanner_object.generic_start_angle
    end_angle=scanner_object.generic_end_angle
    noise_mu = scanner_object.generic_noise_mu
    noise_sigma=scanner_object.generic_noise_sigma
    laser_angles = scanner_object.generic_laser_angles
    rotation_speed = scanner_object.generic_rotation_speed

    inv_scan_x = scanner_object.inv_scan_x
    inv_scan_y = scanner_object.inv_scan_y
    inv_scan_z = scanner_object.inv_scan_z    

    """Standard Error model is a Gaussian Distribution"""
    model = gaussian_error_model.GaussianErrorModel(noise_mu, noise_sigma)
    if scanner_object.generic_advanced_error_model:
      """Advanced error model is a list of distance,mu,sigma tuples"""
      model = advanced_error_model.AdvancedErrorModel(scanner_object.generic_advanced_error_model)


    start_time = time.time()

    current_time = simulation_time
    delta_rot = angle_resolution*math.pi/180

    evd_storage = evd.evd_file(evd_file)

    xaxis = Vector([1,0,0])
    yaxis = Vector([0,1,0])
    zaxis = Vector([0,0,1])

    rays = []
    ray_info = []

    angles = end_angle-start_angle
    steps_per_rotation = angles/angle_resolution
    time_per_step = (1.0/rotation_speed) / steps_per_rotation

    lines = (end_angle-start_angle)/angle_resolution

    laser_angles = angles_from_string(laser_angles)


    rays = []
    ray_info = []

    #Bad code???
    #steps_per_rotation = 360.0/angle_resolution
    #time_per_step = (1.0 / rotation_speed) / steps_per_rotation
    #angles = end_angle-start_angle
  
    lines = (end_angle-start_angle)/angle_resolution
    ray = Vector([0.0,0.0,0.0])
    for line in range(int(lines)):
        for laser_idx in range(len(laser_angles)):
            ray.xyz = [0,0,max_distance]
            rot_angle = 1e-6 + start_angle+float(line)*angle_resolution + 180.0
            timestamp = ( (rot_angle-180.0)/angle_resolution) * time_per_step 
            rot_angle = rot_angle%360.0
            ray_info.append([deg2rad(rot_angle), deg2rad(laser_angles[laser_idx]), timestamp])
            
            rotator = Euler( [deg2rad(-laser_angles[laser_idx]), deg2rad(rot_angle), 0.0] )
            ray.rotate( rotator )
            rays.extend([ray[0],ray[1],ray[2]])


    returns = blensor.scan_interface.scan_rays(rays, max_distance, inv_scan_x = inv_scan_x, inv_scan_y = inv_scan_y, inv_scan_z = inv_scan_z)

    verts = []
    verts_noise = []

    reusable_vector = Vector([0.0,0.0,0.0,0.0])
    if len(laser_angles) != len(laser_noise):
      randomize_distance_bias(len(laser_angles), noise_mu,noise_sigma)
      
    for i in range(len(returns)):
        idx = returns[i][-1]
        reusable_vector.xyzw = [returns[i][1],returns[i][2],returns[i][3],1.0]
        vt = (world_transformation * reusable_vector).xyz
        v = [returns[i][1],returns[i][2],returns[i][3]]
        verts.append ( vt )

        vector_length = math.sqrt(v[0]**2+v[1]**2+v[2]**2)
        distance_noise =  laser_noise[idx%len(laser_noise)] + model.drawErrorFromModel(vector_length) 
        norm_vector = [v[0]/vector_length, v[1]/vector_length, v[2]/vector_length]
        vector_length_noise = vector_length+distance_noise
        reusable_vector.xyzw = [norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
        v_noise = (world_transformation * reusable_vector).xyz
        verts_noise.append( v_noise )

        evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])


    current_angle = start_angle+float(float(int(lines))*angle_resolution)
            
    if evd_file:
        evd_storage.appendEvdFile()
#.........这里部分代码省略.........

# 需要导入模块: from mathutils import Vector [as 别名]
# 或者: from mathutils.Vector import xyzw [as 别名]
def scan_advanced(scanner_object, max_distance = 120, filename=None, add_blender_mesh = False,
    world_transformation=Matrix()):
    start_time = time.time()

    add_noisy_blender_mesh = scanner_object.add_noise_scan_mesh

    bpy.context.scene.render.resolution_percentage=100
    bpy.context.scene.render.use_antialiasing=False
    width = bpy.context.scene.render.resolution_x
    height = bpy.context.scene.render.resolution_y
    cx = float(bpy.context.scene.render.resolution_x) /2.0
    cy = float(bpy.context.scene.render.resolution_y) /2.0 



    flength = 35 #millimeters
    if bpy.context.scene.camera.data.lens_unit == "MILLIMETERS":
        flength = bpy.context.scene.camera.data.lens
    else:
        print ("Lens unit has to be millimeters")
        return False, 0.0,0.0

    focal_length = flength * blensor.globals.getPixelPerMillimeter(width,height)

    bpy.ops.render.render()

    zbuffer = bpy.data.images["Render Result"].zbuf()
    depthmap = [0.0]*len(zbuffer)

    verts = []

    reusable_vector = Vector([0.0,0.0,0.0,0.0])
    for idx in range( len(zbuffer) ):
            x = float(idx % width)
            y = float(idx // width)
            dx = x - cx
            dy = y - cy

            ddist = math.sqrt(dx**2 + dy**2)
            

            world_ddist = ( ddist * zbuffer[idx] ) / focal_length


            object_distance =  math.sqrt(world_ddist ** 2 + zbuffer[idx] ** 2)
            
            depthmap[idx] = object_distance
            if add_blender_mesh or add_noisy_blender_mesh:
                if object_distance < max_distance:
                    Z = -zbuffer[idx] 
                    X = -( Z * dx ) / focal_length
                    Y = -( Z * dy ) / focal_length
                    reusable_vector.xyzw = [X,Y,Z,1.0]
                    vt = (world_transformation * reusable_vector).xyz

                    verts.append((vt[0],vt[1],vt[2]))                

    if filename:
        fh = open(filename, "w")
        fh.buffer.write(struct.pack("ii",width,height))
        for idx in range( width*height ):
            fh.buffer.write(struct.pack("d", depthmap[idx]))
        fh.close()

    if add_blender_mesh:
        mesh_utils.add_mesh_from_points_tf(verts, "Scan", world_transformation)

    if add_noisy_blender_mesh:
        mesh_utils.add_mesh_from_points_tf(verts, "NoisyScan", world_transformation)
        
    bpy.context.scene.update()

    end_time = time.time()
    scan_time = end_time-start_time
    print ("Elapsed time: %.3f"%(scan_time))

    return True, 0.0, scan_time