BlueBotics控制器及其地图存储格式

  BlueBotics通过SLAM建立的地图被保存为后缀名为map2的文件格式。地图文件中存储的是表示环境墙壁的一段段线段,而不是像ROS中的占据栅格或者点云等形式,这就导致地图文件非常小,仅有几KB。与之相比,SICK的NAV-LOC系列SLAM产品保存的地图则是占据栅格的形式。

  我们可以用记事本打开BlueBotics的地图,内容如下:

Description "" ~
Id 101 ~
Level 1 ~
Version 12 ~
Bin Localization.Points
    Point id=31 pos=-5.253 6.988 cov=5.625e-03 5.625e-03 1.000e-06 ~
    Point id=32 pos=1.424 9.755 cov=5.625e-03 5.625e-03 1.000e-06 ~
    Point id=33 pos=-5.311 1.452 cov=5.625e-03 5.625e-03 1.000e-06 ~
    Point id=34 pos=4.907 9.733 cov=5.625e-03 5.625e-03 1.000e-06 ~
    Point id=35 pos=-3.544 9.8 cov=5.625e-03 5.625e-03 1.000e-06 ~
    Point id=36 pos=7.707 1.58 cov=5.625e-03 5.625e-03 1.000e-06 ~
~
Bin Localization.Segments
    Segment id=1 p1=7.707 5.064 p2=7.174 5.065 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=2 p1=7.689 4.151 p2=7.696 5.101 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=4 p1=7.564 1.833 p2=7.458 3.99 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=7 p1=7.61 -1.817 p2=7.664 1.992 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=8 p1=7.236 -2.792 p2=7.241 -2.015 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=9 p1=7.172 -4.299 p2=7.159 -3.654 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=10 p1=0.586 6.006 p2=0.562 5.053 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=12 p1=0.583 5.109 p2=-0.259 5.114 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=13 p1=-0.227 5.853 p2=0.617 5.846 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=14 p1=-0.201 5.073 p2=-0.191 5.927 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=19 p1=1.376 -2.999 p2=1.296 -4.287 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=21 p1=0.574 -1.93 p2=0.577 -2.897 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=22 p1=4.02 -9.641 p2=6.722 -9.66 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=24 p1=2.411 -9.619 p2=3.27 -9.627 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=26 p1=-2.56 -1.905 p2=0.685 -1.932 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=28 p1=-4.782 -1.005 p2=-4.803 -1.843 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
    Segment id=30 p1=-4.78 -1.796 p2=-2.903 -1.767 cov1=6.250e-04 6.250e-04 1.000e-04 cov2=6.250e-04 6.250e-04 1.000e-04 ~
~
Bin Navigation.Nodes
    defaultRadius=1
    defaultMaxRadius=1
    curveType=Spline4
    Node id=1015 pose=2.419 2.237 1.5316 rt=1 links=1018 1019 ~
    Node id=1018 pose=0.343 2.237 0 rt=1 links=1015 ~
    Node id=1019 pose=4.813 2.237 0 rt=1 links=1015 ~
    Home node=1015 ~
~

  从其中的数据我们能很容易地猜出地图的形式,首先这是个二维地图,其由30条轮廓线段和6个点组成。我们用Mathematica软件画出图形,如下图所示。图中6个红色点是反光柱形成的点,黑色线段则对应墙壁或立柱。

  其中使用的Mathematica画图程序代码如下:

SetDirectory[NotebookDirectory[]];
data = Import["1.txt", "Table"];
Quiet[data = ToExpression[data];]
seg = data[[;; , 2 ;; 5]];
data = Import["2.txt", "Table"];
Quiet[data = ToExpression[data];]
pts = data[[;; , 2 ;; 3]];
Graphics[{Red, AbsolutePointSize[4], Point[pts], Black, Line@ArrayReshape[#, {2, 2}] & /@ seg}(*,Axes\[Rule]True*)]

发表回复

您的电子邮箱地址不会被公开。 必填项已用 * 标注