利用Gazebo中的立体相机进行定位

ROS版本： ROS Kinetic
操作系统：Ubuntu16.04 LTS

预备知识

上一篇文章介绍了如何建立双目相机模型及其参数。接下来利用这个双目相机进行物体的定位。在使用立体相机进行定位时，我们需要以下几个参数：
1.相机焦距 :flength 单位：pixel(像素）
2.两个相机的距离即基线：baseline 单位：m(米)
3.图像的大小(分辨率)：height x width 单位：pixel(像素）
—理想相机中点坐标分别为cx = wigth/2,cy = height/2
4.我们定位的点在左右目中的图像定位的点是（xl,yl),(xr,yr)(单位：pixel(像素）)

其中，前3个参数在相机固定时就已经是定值（当然对于有畸变的相机，需要相机实际的焦距和中心点cx,xy)。第四个参数需要我们用图像处理的方法得到（后面会涉及）。由以上参数我们就可以得到以下计算相机的深度depth和x,y的位置（相对于左目）：

#立体位置计算
disparity = abs(xl- xr)
depth =   (flength * baseline)/(disparity)
x =  ((xl - cx) * baseline)/(disparity)
y = (( yl - cy) * baseline)/(disparity)

这里只是给出结果，具体的原理分析需要作图分析，后面再写篇文章，网上也有很多相关的介绍和教材可以参考。

定位算法

这里来用一个测试程序来说明结果的正确性，主要由以下步骤。
1.rosrun gazebo_ros gazebo 打开gazeo
2.将上篇文章提到的立体相机模型手动放到原点位置，然后在镜头前不远处放入一个小球，这里我用的模型库里的RoboCup_SPL_Ball,然后打开rqt_image_view查看是否有小球，并查看是否在左右相机中小球视角发生变化。
3.订阅相机发布的图像，处理得到左右目中小球球心的位置，计算（x,y,depth）
4.与gazebo中小球的位置对比（一个格子1m可以直接对比）。
以下是python实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#利用获取的双目图像进行定位
import rospy
import cv2
import message_filters
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg