Graph SLAM 是众多SLAM方法中的一种
约束: 对于6个姿势:
考虑上图,有4个姿势(包括初始位置x0)和一个地标。我们可以用同样的数学方法,对给定的图像有5个总约束。 你可能注意到,不是所有的约束都会提供有用的信息。例如:图中X2没有和地标之间进行测量。
将两次约束相加:
添加地标:
将没有联系的矩阵置为0:
为了实现Graph SLAM ,引入了
,该矩阵为方正,并标有所有机器人姿势
和所有地标
。当在两个姿势移动距离
,将这两个位置关联起来,可以将其表示为这些矩阵中的数值关系。下图为
的一个矩阵表示和
的一个向量表示:
为了确定姿势和地标位置
当机器人移动一定量
,需要更新约束矩阵,如下所示:
xt
和xt+1
交叉点的索引处将 [[1, -1], [-1, 1]]
添加到omega中xt
和xt+1
行中将 -dx
和dx
添加到xi中import numpy as np
def mu_from_positions(initial_pos, move1, move2):
## TODO: construct constraint matrices
## and add each position/motion constraint to them
# Your code here
omega = np.zeros((3,3))
xi = np.zeros((3,1))
omega[0][0] = 1
# account for the first motion, dx = move1
xi[0] = initial_pos
omega += [[1., -1., 0.],
[-1., 1., 0.],
[0., 0., 0.]]
xi += [[-move1],
[move1],
[0.0]]
# account for the second motion
omega += [[0., 0., 0.],
[0., 1., -1.],
[0., -1., 1.]]
xi += [[0.],
[-move2],
[move2]]
# display final omega and xi
print('Omega: \n', omega)
print('\n')
print('Xi: \n', xi)
print('\n')
## TODO: calculate mu as the inverse of omega * xi
## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse
omega_inv = np.linalg.inv(np.matrix(omega))
mu = omega_inv*xi
return mu
# call function and print out `mu`
mu = mu_from_positions(-3, 5, 3)
print('Mu: \n', mu)