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([