Solution #2: Custom Implementation of Graph SLAM

A project log for Multi-Domain Depth AI Usecases on the Edge

SLAM, ADAS-CAS, Sensor Fusion, Touch-less Attendance, Elderly Assist, Monocular Depth, Gesture & Security Cam with OpenVINO, Math & RPi

In order to do SLAM compute, you need to update the values in the 2D Ω matrix and ξ vector, to account for motion and measurement constraints in the x and y directions.

## Optimized implementation of Graph SLAM.

## slam takes in 6 arguments and returns mu,
## mu is the entire path traversed by a robot (all x,y poses) *and* all landmarks locations
def slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise):

coefficients = [1, -1, -1, 1]

# initialize the constraints
initial_omega_1, initial_xi_1, initial_omega_2, initial_xi_2 = \\
initialize_constraints(N, num_landmarks, world_size)

## get all the motion and measurement data as you iterate
for i in range(len(data)):

landmarks = data[i]  # measurement
motion = data[i]  # motion

# setting measurement constraints
for landmark in landmarks:

# calculate indices in the same order as coefficients (to meaningfully add)
index1 = [i, i, N+landmark, N+landmark]
index2 = [i, N+landmark, i, N+landmark]

# dx update
initial_omega_1[index1, index2] = initial_omega_1[index1, index2] + \\
np.divide(coefficients, measurement_noise)
initial_xi_1[[i, N+landmark]] = initial_xi_1[[i, N+landmark]] + \\
np.divide([-landmark, landmark], measurement_noise)

# dy update
initial_omega_2[index1, index2] = initial_omega_2[index1, index2] + \\
np.divide(coefficients, measurement_noise)
initial_xi_2[[i, N+landmark]] = initial_xi_2[[i, N+landmark]] + \\
np.divide([-landmark, landmark], measurement_noise)

index1 = [i, i, i+1, i+1]
index2 = [i, i+1, i, i+1]

# dx update
initial_omega_1[index1, index2] = initial_omega_1[index1, index2] + \\
np.divide(coefficients, motion_noise)
initial_xi_1[[i, i+1]] = initial_xi_1[[i, i+1]] + \\
np.divide([-motion, motion], motion_noise)

# dy update
initial_omega_2[index1, index2] = initial_omega_2[index1, index2] + \\
np.divide(coefficients, motion_noise)
initial_xi_2[[i, i+1]] = initial_xi_2[[i, i+1]] +
np.divide([-motion, motion], motion_noise)

## To update the constraint matrix/vector to account for all measurements,
# measurement noise, motion and motion noise. Compute best estimate of poses
# and landmark positions using the formula, omega_inverse * Xi
mu_1 = np.linalg.inv(np.matrix(initial_omega_1)) * \\
np.expand_dims(initial_xi_1, 0).transpose()
mu_2 = np.linalg.inv(np.matrix(initial_omega_2)) * \\
np.expand_dims(initial_xi_2, 0).transpose()

mu = []
for i in range(len(mu_1)):
mu.extend((mu_1[i], mu_2[i]))

return mu

The complete source code and results of Custom SLAM implementation can be found in the IPython notebook here.

Discussions 