def simulate_random(self,
world_poses_gt,
planes_gt,
move_window=5,
planes_per_pose=5):
num_poses = world_poses_gt.shape[0]
odom_gt, odom_sim = self.simulate_odometry(world_poses_gt)
landmark_measurements = []
count = 0
n = 0
for i in range(num_poses - 1):
noisy_planes = np.vstack([
np.hstack([
transform_plane_to_local(
world_poses_gt[i], add_plane_noise(plane, self.std_l)),
i, j
]) for j, plane in enumerate(planes_gt)
])
if count == move_window:
n += 1
count = 0
landmark_measurements.append(
noisy_planes[n * planes_per_pose:(n + 1) * planes_per_pose, :])
count += 1
return odom_sim, np.vstack(landmark_measurements)
def landmark_model(self, pose_w, planes):
return transform_plane_to_local(pose_w, planes)
std_l = np.array([0.01, 0.01, 0.01, 0.01]) # nx, ny, nz, d
std_p = np.array([0.05, 0.05, 0.001])
init_pose = np.array([0, 0, 0])
obj = SLAMBackend(std_p, std_x, std_l, init_pose)
print(obj.s_x)
print(obj.s_l)
print(obj.odom)
x1 = np.array([2, 3, np.deg2rad(45)])
odom1 = x1 - init_pose
plane1 = np.array([0.707, 0.707, 0, 5]) / np.linalg.norm([0.707, 0.707])
plane2 = np.array([-0.707, 0.707, 0, 2]) / np.linalg.norm([0.707, 0.707])
measurement11 = np.hstack(
[transform_plane_to_local(init_pose, plane1), 0, 0])
measurement12 = np.hstack(
[transform_plane_to_local(init_pose, plane2), 1, 0])
measurement21 = np.hstack([transform_plane_to_local(x1, plane1), 0, 1])
measurement22 = np.hstack([transform_plane_to_local(x1, plane2), 1, 1])
landmark_measurements = np.vstack(
[measurement11, measurement12, measurement21, measurement22])
obj.add_landmark_measurement(landmark_measurements[:2, :])
print(obj.s_l)
print(obj.landmarks)
obj.add_pose_measurement(odom1)
obj.add_landmark_measurement(landmark_measurements[2:, :])
print(obj.s_x)
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([
transform_plane_to_local(gt[i], add_plane_noise(plane, std_l)),
i, j
]) for j, plane in enumerate(planes_gt)
])
if count == 5:
n += 1
count = 0
# obj.add_landmark_measurement(landmark_measurements[n:n+10,:])
# obj.add_landmark_measurement(landmark_measurements[n:(n+1)*5,:])
obj.add_landmark_measurement(landmark_measurements)
print(obj.s_l.shape)
count += 1
# import pdb; pdb.set_trace()
obj.solve()
def get_noisy_planes(self, pose, plane, l_idx, p_idx):
return np.hstack([
transform_plane_to_local(pose, add_plane_noise(plane, self.std_l)),
p_idx, l_idx
])