def main_pc(): # this code uses pictures taken manualy # this is not the correct way to use the code but because of COVID-19, the setup was not obtainable folder = 'D:\_Udes\S4\Projet\ScanUS\Photos_boite/' files = os.listdir(folder) laser_angles = [-5, -3, -2, -1, -0.5, 0, 0.5, 1.5, 2, 3, 4] my_plot = plot.Plot(name='main plot', range=[0, 10]) position_laser_ref_plaque = Matrix(pos=[259, 512, 150]) angle_laser_cam = 11.1 * 2 * math.pi / 360 + math.atan(259 / 512) trans_plaque_to_cam_ref = Matrix(angles=[0, 0, angle_laser_cam]) position_laser = mult([trans_plaque_to_cam_ref, position_laser_ref_plaque]) for i in range(len(files)): filename = folder + files[i] img = cv2.imread(filename) x, y, fail = camera.find_red_dot(frame=img) if not fail: angle_table = ((i * 11.25) % 360) * 2 * math.pi / 360 angle_laser = laser_angles[i // 32] * 2 * math.pi / 360 p1, v1 = camera.get_red_dot_point_vector_in_world( angle_table, x, y) p2, v2 = laser.get_laser_point_vector_in_world( angle_table=angle_table, angle_wrist=angle_laser) p, error = intersect(p1, v1, p2, v2) my_plot.add_point(p + [error]) if i > 11 * 32: break input() my_plot.close()
def get_camera_ext_matrix(angle_table=0, cam_pos=CAMERA_POS, cam_angles=CAMERA_ANGLES): # The camera ext matrix should have its coordinate system with # the y axis pointing through the center of the image floor_matrix = Matrix(angles=[0, 0, angle_table]) cam_matrix = Matrix(pos=cam_pos, angles=cam_angles) result_matrix = mult([floor_matrix, cam_matrix]) return result_matrix
def test_Matrix(self): # expected answers where hand calculated pos = [1, 2, 3] angles = [0, 0, pi / 2] expected_angles = [[0, -1, 0], [1, 0, 0], [0, 0, 1]] np.testing.assert_allclose( Matrix(pos=pos, angles=angles).get_pos(), pos) np.testing.assert_allclose(Matrix(pos=pos, angles=angles).get_angle_matrix(), expected_angles, atol=0.01)
def test_rotate_4(self): start = [ [1, 2, 1], [2, 2, 2], [2, 1, 2], ] m = Matrix(start) rotated = m.rotate_n(4) end = [ [1, 2, 1], [2, 2, 2], [2, 1, 2], ] self.assertEqual(rotated, end)
def test_mult(self): # expected answers where hand calculated ma = Matrix(pos=[1, 2, 3], angles=[0, -pi / 2, 0]) mb = Matrix(pos=[0, 0, 0], angles=[0, 0, pi / 2]) expected_a_b = np.array([[0, 0, -1, 1], [1, 0, 0, 2], [0, -1, 0, 3], [0, 0, 0, 1]]) expected_b_a = np.array([[0, -1, 0, -2], [0, 0, -1, 1], [1, 0, 0, 3], [0, 0, 0, 1]]) np.testing.assert_allclose(mult([ma, mb]).matrix, expected_a_b, atol=0.01) np.testing.assert_allclose(mult([mb, ma]).matrix, expected_b_a, atol=0.01)
def get_laser_point_vector_in_world(angle_table=0, angle_tower=0, angle_wrist=0): laser_matrix_world = get_laser_line_in_world(angle_table, angle_tower, angle_wrist) point = laser_matrix_world.get_pos() unit_vector_matrix = Matrix(pos=[0, 1, 0]) point_vector_direction = mult([laser_matrix_world, unit_vector_matrix]) vector = point_vector_direction.get_pos() - point return point, vector
def get_red_dot_point_vector_in_world(angle_table=0, x_image=0, y_image=0, cam_pos=CAMERA_POS, cam_angles=CAMERA_ANGLES): m_cam_ext = get_camera_ext_matrix(angle_table, cam_pos, cam_angles) point = m_cam_ext.get_pos() v_red_dot_cam_ref = get_red_dot_vector_from_cam(x_image, y_image) m_red_dot_cam_ref = Matrix(pos=v_red_dot_cam_ref) m_red_dot_world_ref = mult([m_cam_ext, m_red_dot_cam_ref]) p_red_dot_world_ref = m_red_dot_world_ref.get_pos() p_cam_world_ref = m_cam_ext.get_pos() vector = list(p_red_dot_world_ref - p_cam_world_ref) return point, vector
def main_pi(): # This code is obsolete # This code needs to be changed with functions from laser.py if the actual laser tower is used my_plot = plot.Plot(name='main plot', range=[0, 10]) laser_p = [0, 0, 180] with camera.init_picamera() as cam: input() motor.start_motor() temp = 0 while (True): pic = camera.take_one_picture_pi(cam) # print(pic) ang = -motor.get_angle_motor() x, y = camera.find_red_dot(pic) if ang > temp: break floor_matrix = Matrix(angles=[0, 0, ang]) cam_matrix = Matrix(pos=laser_p, angles=[0, 0, 0.26]) result_matrix = mult([floor_matrix, cam_matrix]) p2 = result_matrix.get_pos() v2 = result_matrix.get_vector_in_referential([0, 1, 0]) p1, v1 = camera.get_red_dot_point_vector_in_world(ang, x, y) m, l = intersect(p1, v1, p2, v2) point = m + [l] my_plot.add_point(point) # print(m, l, sep=' ') temp = ang # k = input() # if k == 'q': # break motor.restart_motor() input() my_plot.close()
def get_laser_line_in_world(angle_table=0, angle_tower=0, angle_wrist=0): floor_matrix = Matrix(angles=[0, 0, angle_table]) wrist_matrix = get_wrist_matrix(angle_wrist) tower_matrix = get_tower_matrix(angle_tower) laser_matrix = mult([floor_matrix, tower_matrix, wrist_matrix]) return laser_matrix
def get_tower_matrix(angle_tower=0): height = angle_to_dist(angle_tower) fixed_matrix = Matrix(pos=TOWER_POS, angles=TOWER_ANGLES) variable_matrix = Matrix(pos=[0, 0, height]) return mult([fixed_matrix, variable_matrix])
def get_wrist_matrix(angle_wrist=0): fixed_matrix = Matrix(pos=WRIST_POS, angles=WRIST_ANGLES) variable_matrix = Matrix(angles=[angle_wrist, 0, 0]) return mult([fixed_matrix, variable_matrix])