首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >Simultaneous localization and Mapping

Simultaneous localization and Mapping

作者头像
小飞侠xp
发布2018-11-12 16:42:48
6650
发布2018-11-12 16:42:48
举报
Graph SLAM

Graph SLAM 是众多SLAM方法中的一种

约束: 对于6个姿势:

  • 一初始位置约束
  • 5个附加相对运动约束
  • 8个地标位置相对测量约束 所有这些加起来总共14个约束

考虑上图,有4个姿势(包括初始位置x0)和一个地标。我们可以用同样的数学方法,对给定的图像有5个总约束。 你可能注意到,不是所有的约束都会提供有用的信息。例如:图中X2没有和地标之间进行测量。

将两次约束相加:

添加地标:

将没有联系的矩阵置为0:

\Omega 与\xi
\Omega 与\xi

为了实现Graph SLAM ,引入了

\Omega 与\xi
\Omega 与\xi

,该矩阵为方正,并标有所有机器人姿势

Xi
Xi

和所有地标

Li
Li

。当在两个姿势移动距离

dx
dx

,将这两个位置关联起来,可以将其表示为这些矩阵中的数值关系。下图为

\Omega
\Omega

的一个矩阵表示和

Xi
Xi

的一个向量表示:

为了确定姿势和地标位置

\mu = \Omega^{-1}\xi
\mu = \Omega^{-1}\xi
  • 初始位置 -3
  • 移动 5
  • 移动 3
约束更新

当机器人移动一定量

dx
dx

,需要更新约束矩阵,如下所示:

  • xtxt+1交叉点的索引处将 [[1, -1], [-1, 1]]添加到omega中
  • xtxt+1行中将 -dxdx添加到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)
本文参与 腾讯云自媒体分享计划,分享自作者个人站点/博客。
原始发表:2018.10.23 ,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 作者个人站点/博客 前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体分享计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • Graph SLAM
  • 约束更新
领券
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档