利用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 import Image
import math #计算正切

bridge = CvBridge()
hfov = 1.3962634
image_width = 800
flength = (image_width/2) / ( math.tan(hfov/2) )
base_line = 0.2
cx = 400
cy = 400
def callback(left_ros_image,right_ros_image):
    global flength
    global cx 
    global cy

    left_cv_image = bridge.imgmsg_to_cv2(left_ros_image)
    right_cv_image = bridge.imgmsg_to_cv2(right_ros_image)
    #图像处理获取参考点图像坐标
    grayl = cv2.cvtColor(left_cv_image, cv2.COLOR_BGR2GRAY) #灰度化
    grayr = cv2.cvtColor(right_cv_image, cv2.COLOR_BGR2GRAY)
    ret1l,andMaskBil = cv2.threshold(grayl, 100, 255, cv2.THRESH_BINARY_INV); #进行阈值操作,这里针对其颜色做了一个反转_INV
    ret1r,andMaskBir = cv2.threshold(grayr, 100, 255, cv2.THRESH_BINARY_INV); 
            
    ret2l,contoursl, hierarchyl = cv2.findContours(andMaskBil.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)#轮廓查找注意此算法当背景为白色物体为黑色时，可能使得最后的轮廓只有边框
    ret2r,contoursr, hierarchyr = cv2.findContours(andMaskBir.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    cntl = contoursl[0] #取该条轮廓,默认图像中只有一个轮廓，所以视野中只放一个小球
    Ml = cv2.moments(cntl)#计算轮廓的矩
    cxl_f=((Ml['m10']/Ml['m00'])) #浮点数表示的质心横坐标  
    cyl_f=((Ml['m01']/Ml['m00'])) #浮点数表示的质心纵坐标



    cntr = contoursr[0] #取该条轮廓
    Mr = cv2.moments(cntr)#计算轮廓的矩
    cxr_f=((Mr['m10']/Mr['m00'])) #浮点数表示的质心横坐标 
    cyr_f=((Mr['m01']/Mr['m00'])) #浮点数表示的质心纵坐标
   
    disparity = abs(cxl_f - cxr_f)
    depth =   (flength * base_line)/(disparity)
    x =  ((cxl_f - cx) * base_line)/(disparity)
    y = (( cyl_f - cy) * base_line)/(disparity)
    print(x)
    print(y)
    print(depth)
   #图像显示
    cv2.imshow('left_image',left_cv_image)
    cv2.imshow('andMaskBil',andMaskBil)
    key =cv2.waitKey(30)


if __name__ == '__main__':
    rospy.init_node('gazebo_image_sub', anonymous=True)

    left_ros_image = message_filters.Subscriber("/multisense_sl/camera/left/image_raw", Image)
    right_ros_image =message_filters.Subscriber("/multisense_sl/camera/right/image_raw", Image)
    ts = message_filters.TimeSynchronizer([left_ros_image , right_ros_image], 10)
    ts.registerCallback(callback)
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()