ROS-根据map.yaml进行像素坐标和map坐标的转换

根据map.yaml进行地图坐标与像素坐标的转换

应用背景说明

在我们使用SLAM时，输出的是基于地图坐标系下坐标，有时候，我们需要将该坐标在图片上进行展示。
或则通过图片设置一个点位，供机器人导航至该目标点。
因此，需要图片像素坐标和地图坐标进行相互转换。

地图文件说明

在使用二维地图定位导航时，建好的地图文件中包括map.pgm和map.yaml

其中,yaml文件如下

image: map.pgm　　#文件名
resolution: 0.050000　　#地图分辨率 单位：米/像素
origin: [-49.0286, -107.401, 0.0]   #图像左下角在地图坐标下的坐标
negate: 0    #是否应该颠倒 白：自由/黑：的语义(阈值的解释不受影响)
occupied_thresh: 0.65   #占用概率大于此阈值的像素被认为已完全占用
free_thresh: 0.196   #用率小于此阈值的像素被认为是完全空闲的

配置中origin表示了建图起始点
即：图像左下角在地图坐标系下的坐标

如下图所示
左下角原点在地图坐标下的坐标即为[-49.0286, -107.401, 0.0]

由地图坐标->图像像素坐标

基于地图的坐标转换到图像坐标系上
wx wy 表示地图坐标系下的坐标，resolution为分辨率,则

image_x = (wx - origin_x) / resolution
image_y = (wy - origin_y) / resolution

如上面的地图原点坐标在像素坐标下
x=(0-(-49.0286))/0.05=980.56
y=(0-(–107.401))/0.05=2148.02

由图像像素坐标->地图坐标

image_x,image_y表示在图像像素坐标系中的坐标
wx wy 表示地图坐标系下的坐标，resolution为分辨率,则

wx=image_x*resolution+origin_x
wy=image_y*resolution+origin_y