def generate_circular_movement_gt(num_poses, x_movement, y_movement, theta_deg, num_planes): gt_odom = generate_circular_movement_odom(num_poses, x_movement, y_movement, theta_deg) init_pose = np.array([0, 0, 0]) world_poses_gt = [init_pose] for i, odom in enumerate(gt_odom): world_poses_gt.append(geom.compute_world_pose(world_poses_gt[-1], odom)) world_poses_gt = np.vstack([world_poses_gt]) midpoints = None size = None planes_gt = geom.normalize_planes( np.hstack([ np.random.randint(2, 10, (num_planes, 2)), np.zeros((num_planes, 1)), np.random.randint(50, 100, (num_planes, 1)) ])) plane1 = np.array([0.707, 0.707, 0, 5]) plane2 = np.array([-0.707, 0.707, 0, 2]) plane3 = np.array([2, 1, 0, 2]) plane4 = np.array([3, 4, 0, 2]) plane5 = np.array([5, 6, 0, 2]) #planes_gt = geom.normalize_planes(np.vstack([plane1, plane2, plane3, plane4, plane5])) plane_data = planes_gt, midpoints, size return plane_data, world_poses_gt
def generate_room_gt(): hexagon1_pts, hexagon2_pts = create_hexagons() planes, midpoints, size = generate_room_planes(hexagon1_pts, hexagon2_pts) planes[planes[:, 3] < 0] *= -1 planes = geom.normalize_planes(planes) world_poses_gt = generate_room_odom() plane_data = planes, midpoints, size return plane_data, world_poses_gt
def plot_planes(planes_p, planes_gt, midpoints, size=2): # line from intesrsection of plane and z= 0 plane is composed of indices # 0, 1, 3. ax + by + c = 0 planes_p = geom.normalize_planes(planes_p) edge_pts1, edge_pts2 = find_line_segment_edge_points( planes_p[:, [0, 1, 3]], midpoints, size) plot_lines(edge_pts1, edge_pts2, "Planes Predicted", 'r') edge_pts1_gt, edge_pts2_gt = find_line_segment_edge_points( planes_gt[:, [0, 1, 3]], midpoints, size) plot_lines(edge_pts1_gt, edge_pts2_gt, "Planes Ground Truth", 'k')
def add_plane_noise(planes, std): normalized_planes = normalize_planes( (planes + measurement_noise(std)).reshape(1, -1)) return normalized_planes.reshape(-1, )
x.append(compute_world_pose(x[-1], odom)) x = np.vstack([x]) std_x = np.array([0.3, 0.3, .01]) # x, y, theta std_l = np.array([0.01, 0.01, 0.01, 0.005]) # nx, ny, nz, d std_p = np.array([0.05, 0.05, 0.001]) plane1 = np.array([0.707, 0.707, 0, 5]) plane2 = np.array([-0.707, 0.707, 0, 2]) plane3 = np.array([2, 1, 0, 2]) # planes_gt = normalize_planes(np.vstack([plane1, plane2, plane3])) num_planes = 20 planes_gt = normalize_planes( np.hstack([ np.random.randint(2, 10, (num_planes, 2)), np.zeros((num_planes, 1)), np.random.randint(50, 100, (num_planes, 1)) ])) print(planes_gt) obj = SLAMBackend(std_p, std_x, std_l, init_pose) plane_map = {i: -1 for i in range(num_planes)} count = 0 n = 0 for i, odom in enumerate(odom_list): print("Iteration:", i) obj.add_pose_measurement(odom) # see all planes always landmark_measurements = np.vstack([ np.hstack([