Gazebo中加入一个双目相机及相机参数设置

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

kinect

为了在gazebo中观察并利用利用视觉进行小球的定位并与ros通信，我们需要一个深度相机，这时可以直接使用kinect并加入插件与ros通信,gazebo官方也为我们提供了这个方法
http://gazebosim.org/tutorials/?tut=ros_depth_camera
直接将Kinect的模型文件下载在

<sensor>
....
</camera>
在此处添加插件代码即可
</sendor>

但有时，我们需要得到左右目两张图像，这是，我们就需要一个双目相机模型。
##双目相机
方法类似，我们可以继续使用kinect的外观，使用一个Muticamera插件，
http://gazebosim.org/tutorials?tut=ros_gzplugins
这时只需要将上述文件的sensor部分替换为官方给出的sensor部分的代码即可。这时在rqt_image_view中可以看到left和right两张图像

注意：以上两种修改sdf文件的方法不要忘记修改模型文件夹名字，sdf文件中的model名字，以及config文件中的model名

显示

首先，用rosrun gazebo_ros gazebo命令打开gazebo之后，在新的终端输入rostopic list,我们可以看到左右目的话题都在发布，主要是以下两个话题，

/multisense_sl/camera/left/image_raw
/multisense_sl/camera/left/image_raw

我们的目的是订阅这两个话题并显示，我们先尝试订阅一个话题

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#订阅gazebo中双目相机左目的图像并显示
import rospy
import cv2
import message_filters
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
#ros传感器和opencv中的图像类型转换
bridge = CvBridge()
#回调函数
def callback(left_ros_image):
    left_cv_image = bridge.imgmsg_to_cv2(left_ros_image)
    cv2.imshow('left_image',left_cv_image)
    key =cv2.waitKey(1)

if __name__ == '__main__':
    #初始化节点
    rospy.init_node('gazebo_image_sub', anonymous=True)
    #订阅左目图像
    left_ros_image = rospy.Subscriber("/multisense_sl/camera/left/image_raw", Image,callback)
    rospy.spin()

注意，这前两行别忘了复制，不然可能会出现编码错误

#!/usr/bin/env python
# -*- coding: utf-8 -*-

ok,接下来我们显示双目图像，我们这里用到ros的一个message_filter的包，里面有一个Time Synchronizer，可以将接收到的信息进行同步。
https://wiki.ros.org/message_filters#Time_Synchronizer

19.12.17补充：采用这种方式发现程序经常运行一会儿就会卡住，后来发现是cv2.imshow()的问题，将其注释掉，就不会卡住了。具体原因还没找到。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#订阅gazebo中双目相机的图像并显示
import rospy
import cv2
import message_filters
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

bridge = CvBridge()


def callback(left_ros_image,right_ros_image):
    left_cv_image = bridge.imgmsg_to_cv2(left_ros_image)
    right_cv_image = bridge.imgmsg_to_cv2(right_ros_image)
    cv2.imshow('left_image',left_cv_image)
    cv2.imshow('right_image',right_cv_image)
    key =cv2.waitKey(1)


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()

插件参数

ok,现在我们想试一下这个立体相机能不能工作，我们需要知道我们到底对这个相机做了什么。介绍这部分之前，可以先看看有关相机镜头畸变和矫正相关的内容。这里在代码中对其进行注释。以下是整个立体相机的module.sdf文件。

  
    True
    0 0 0.2 0 0 3.14

开始module标签定义了模型的名字，pose标签指定初始位置，这里我理解的是相对于gazebo中的世界坐标系。static可省略，这里让其为True是想让相机不借助其他模型悬浮在空中。

    
      
        0.1
      
      
        
          
            0.073000 0.276000 0.072000
          
        
      
      
        
          
            model://kinect/meshes/kinect.dae
          
        
      

以上主要是三个标签，inertial中定义了模型质量（但由于我们将static设置为True这里相机将不受重力影响）。collision定义其物理属性，指的是在和其他物体碰撞时，以geometry中的属性进行碰撞。而visual标签中的geometry是指视觉效果。（我目前的理解是这样，如果visual中定以为球，而collision中定义为正方体，给这个物体一个力他会滑动，而不是滚动。因为collison中定义的是物理形状，而visual只是视觉形状）

      
      30.0
      
        1.3962634
        
          800
          800
          R8G8B8
        
        
          0.02
          300
        
        
          gaussian
          0.0
          0.007
        
      
      
        0 -0.07 0 0 0 0
        1.3962634
        
          800
          800
          R8G8B8
        
        
          0.02
          300
        
        
          gaussian
          0.0
          0.007
        
      

以上是两个传感器，我们可以看到是两个相同的传感器，其定义的属性从标签上就能看出来，这里重点是三个标签，clip中定义了相机能看到的最远和最近位置（现实中因为你焦点的原因，相机看物体模糊但也不至于看不到，所以搞不太懂这个标签的物理意义）。nosie是像素上的噪声，结合实际相机，很容易理解。最后重点说一下pose我们发现left相机是没有pose标签的，默认为0 0 0 0 -0 0，而right相机有个-0.07，负号说明其在右边，0.07就是指的基线的距离。(在这里这个pose还不知道具体是什么，是相机坐标系的原点，还是图像坐标系的远点，在深度上相差一个焦距的距离。关于这两个坐标系，见
https://blog.csdn.net/chentravelling/article/details/53558096)
这个是我们进行立体计算时需要用到的。这时我们发现对于一个镜头来说还需要一个很重要的参数，那就是焦距。但这里并没有给出，实际上我们可以根据水平视场horizontal_fov来计算出来的，参考以下两个介绍：
http://playerstage.sourceforge.net/wiki/GazeboProblemResolutionGuide
https://answers.ros.org/question/12658/how-to-change-gazebo-gui-focal-lengthfov/
即可以通过以下公式得到焦距：
flength = (width/2)/tan(hfov/2）

      
        true
        0.0
        multisense_sl/camera
        image_raw
        camera_info
        left_camera_optical_frame
        
        0.07
        0.0
        0.0
        0.0
        0.0
        0.0
      
    
    
  

以上是传感器插件，我们发现和单目相机的插件很像，主要说下最后几个参数，如果做过单目相机矫正就很容易理解（K1 K2 T1 T2 K3）是相机的畸变系数，这里使其为0，即认为相机没有畸变，理想化了。hackBaseline这里现在也没太搞懂是啥，需要再研究一下。