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

Posted on

kinect

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

1
2
3
4
5
<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,我们可以看到左右目的话题都在发布，主要是以下两个话题，

1
2
/multisense_sl/camera/left/image_raw
/multisense_sl/camera/left/image_raw

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#!/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()

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
#!/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文件。

1
2
3
4
5
<?xml version="1.0" ?>
<sdf version="1.5">
  <model name="Stere_camera">
    <static>True</static>
    <pose>0 0 0.2 0 0 3.14</pose>

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
<link name="link">
  <inertial>
    <mass>0.1</mass>
  </inertial>
  <collision name="collision">
    <geometry>
      <box>
        <size>0.073000 0.276000 0.072000</size>
      </box>
    </geometry>
  </collision>
  <visual name="visual">
    <geometry>
      <mesh>
        <uri>model://kinect/meshes/kinect.dae</uri>
      </mesh>
    </geometry>
  </visual>

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
<sensor type="multicamera" name="stereo_camera">
<update_rate>30.0</update_rate>
<camera name="left">
  <horizontal_fov>1.3962634</horizontal_fov>
  <image>
    <width>800</width>
    <height>800</height>
    <format>R8G8B8</format>
  </image>
  <clip>
    <near>0.02</near>
    <far>300</far>
  </clip>
  <noise>
    <type>gaussian</type>
    <mean>0.0</mean>
    <stddev>0.007</stddev>
  </noise>
</camera>
<camera name="right">
  <pose>0 -0.07 0 0 0 0</pose>
  <horizontal_fov>1.3962634</horizontal_fov>
  <image>
    <width>800</width>
    <height>800</height>
    <format>R8G8B8</format>
  </image>
  <clip>
    <near>0.02</near>
    <far>300</far>
  </clip>
  <noise>
    <type>gaussian</type>
    <mean>0.0</mean>
    <stddev>0.007</stddev>
  </noise>
</camera>

以上是两个传感器，我们可以看到是两个相同的传感器，其定义的属性从标签上就能看出来，这里重点是三个标签，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）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
      <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>multisense_sl/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>left_camera_optical_frame</frameName>
        <!--<rightFrameName>right_camera_optical_frame</rightFrameName>-->
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    </link>
  </model>
</sdf>

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