ROS进阶（三）：机器视觉项目实战

声明：ROS进阶教程参考了《ROS机器人开发实践》，由胡春旭老师所著。此系列教程唯一目的是学习与分享，与大家共勉。若感兴趣，请大家购买原版书籍进行学习，侵权删。

本文在Ubuntu18.04 + ROS melodic环境下完成，其他ROS版本类似。

项目github链接：

1027243334/robot_vision​github.com/1027243334/robot_vision

一、项目介绍

1、项目内容

了解机器人常用视觉传感器：USB摄像头和RGB-D摄像头，通过ROS提供的工具将图像数据显示出来，并实现常用机器视觉应用。

2、预期目标

  • 学习摄像头标定的意义与运用。
  • 学习基于OpenCV的人脸识别和物体跟踪。
  • 学习二维码识别。

二、摄像头简介

1、USB摄像头

USB摄像头是最普遍的摄像头，如笔记本内置的摄像头，在ROS中使用这类设备很简单，可以直接使用usb_cam功能包驱动，USB摄像头输出的是二维图像数据。

（1）usb_cam功能包

usb_cam是针对V4L协议USB摄像头的ROS驱动包，核心节点是usb_cam_node，相关话题和参数如下表所示。

  • 话题
名称 类型 描述
话题发布 ~<camera_name>/image sensor_msgs/Image 发布图像数据
  • 参数
参数 类型 默认值 描述
~video_device string "/dev/video0" 摄像头设备号
~image_width int 640 图像横向分辨率
~image_height int 480 图像纵向分辨率
~pixel_formal string "mjpeg" 像素编码，可选值为"mjpeg、yuyv、uyvy"
~io_method string "mmap" IO通道，可选值"mmap、read、userptr"
~camera_frame_id string "head_camera" 摄像头坐标系
~framerate int 30 帧率
~brightness int 32 亮度：0~255
~saturation int 32 饱和度：0~255
~contrast int 32 对比度：0~255
~sharpness int 22 清晰度：0~255
~autofocus bool false 自动对焦
~focus int 51 焦点（非自动对焦下有效）
~camera_info_url string —— 摄像头校准文件路径
~camera_name string "head_camera" 摄像头名称
  • 安装usb_cam功能包
$ sudo apt-get install ros-melodic-usb-cam

（2）PC端驱动摄像头

安装完usb_cam功能包后，我们可以启动摄像头并显示图像信息，启动摄像头的launch文件可用usb_cam功能包下的usb_cam-test.launch，代码如下：

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw" />
    <param name="autosize" value="true" />
  </node>
</launch>

在运行usb_cam.launch时，摄像头先启动usb_cam_node，并配置相应参数，然后运行image_view节点订阅图像话题/usb_cam/image_raw，将摄像头看到的世界可视化地呈现出来。

运行如下命令启动摄像头：

$ roslaunch usb_cam usb_cam-test.launch

启动成功后弹出图像可视化界面：

使用摄像头的pixel_formal默认参数（mjpeg）有可能启动失败，因为有些摄像头是yuyv格式的，需要在launch文件中修改该参数。

除此之外，还可以使用Qt工具箱显示图像：

$ rqt_image_view

打开界面后，订阅图像话题/usb_cam/image_raw即可显示图像。

2、RGB-D摄像头

除了普通的USB摄像头，很多应用场景下还会用到RGB-D摄像头来获取更丰富的环境信息，如Kinect传感器，它可以获取环境的三维信息。

（1）freenect_camera功能包

Kinect在Linux下有两种开源的驱动包，即OpenNI和Freenect。ROS针对这两个驱动也有相应的功能包：openni_camera和freenect_camera，这里我们使用freenect_camera功能包。

  • 话题与服务
  • 参数
  • 安装功能包
$ sudo apt-get install ros-melodic-freenect-*

在PC或ARM板上运行时，Kinect连接正常的情况下可能会出现找不到日志设备的提示，如下图所示：

这是驱动识别异常导致，需要手动下载驱动并安装：

$ git clone https://github.com/avin2/SensorKinect.git
$ cd SensorKinect/Bin
$ tar xvf SensorKinect093-Bin-Linux-x86-v5.1.2.1.tar.bz2
$ sudo ./install.sh

（2）PC端驱动Kinect

将Kinect连接到PC端的USB接口，然后使用lsusb命令查看是否连接成功，如下图所示，在打印的信息中可以找到Kinect相关信息。

freenect_camera功能包的launch文件夹下包含一个驱动节点的启动文件freenect.launch，运行该文件可以直接驱动Kinect传感器，为了方便节点参数的配置，最好还是单独创建一个launch文件来启动Kinect，这里创建的robot_vision/launch/freenect.launch文件内容如下：

<launch>
    <!-- 启动freenect驱动 -->
    <include file="$(find freenect_launch)/launch/freenect.launch">
        <arg name="publish_tf"                      value="true" /> 

        <!-- use device registration -->
        <arg name="depth_registration"              value="true" /> 

        <arg name="rgb_processing"                  value="true" />
        <arg name="ir_processing"                   value="false" />
        <arg name="depth_processing"                value="false" />
        <arg name="depth_registered_processing"     value="true" />
        <arg name="disparity_processing"            value="false" />
        <arg name="disparity_registered_processing" value="false" />
        <arg name="sw_registered_processing"        value="false" />
        <arg name="hw_registered_processing"        value="true" />
    </include>
</launch>

这个launch文件配置了一些Kinect参数，其中一个参数是“depth_registration”，设置这个选项为“true”后，在驱动Kinect时会把深度数据和摄像头数据进行配准，尽量保持每个像素点的深度信息和图像数据一致，减少偏移。

运行freenect.launch文件，启动Kinect：

$ roslaunch robot_vision freenect.launch

启动后无法直观判断是否正常启动，可以使用ROS中的rviz可视化工具查看Kinect发布的三维数据。

$ rosrun rviz rviz

修改“Fixed Frame”参数为“camera_rgb_optical_frame”，添加一个“PointCloud2”类型的插件，订阅话题“cameara/depth_registered_points”，即可观察到点云数据。

从点云信息中，可以清晰看到Kinect眼中的三维世界与人眼的反馈信息类似，不仅包括颜色信息，而且包含每个像素点的深度信息。你还可以调整点云的信息格式，如选择Color Transformer为AxisColor，通过颜色表示点云的深度。点云数据变成如下图所示的效果。

三、ROS中的图像数据

无论是USB摄像头还是RGB-D摄像头，发布的图像数据格式多种多样，在处理它们之前，先让我们了解清楚这些数据格式。

1、二维图像数据

连接USB摄像头到PC端，通过如下命令启动摄像头：

$ roslaunch robot_vision usb_cam.launch

启动成功后，使用以下命令查看系统中的图像话题信息：

$ rostopic info /usb_cam/image_raw

从上图打印的消息可以看到，图像话题的消息类型是sensor_msgs/Image，这是ROS定义的一种摄像头原始图像的消息类型，可以使用以下命令查看该图像消息的详细定义：

$ rosmsg show sensor_msgs/Image

该消息类型具体内容如下：

  • header：消息头，包含图像的序号、时间戳和绑定坐标系。
  • height：图像的纵向分辨率。
  • width：图像的横向分辨率。
  • encoding：图像的编码格式，包含RGB、YUV等常用格式，不涉及图像压缩编码。
  • is_bigendian：图像数据的大小端存储模式。
  • step：一行图像数据的字节数量，作为数据的步长参数，这里使用的摄像头为 [公式] 字节。
  • data：存储图像数据的数组，大小为 [公式] 字节，根据该公式可以计算出摄像头产生一帧图像的数据大小是： [公式] 字节，即2.7648MB。

一帧 720×1280分辨率的图像数据量就是2.7648MB，如果按照30帧/秒的帧率计算，那么一秒钟摄像头产生的数据量就高达82.944MB！这个数据量在实际应用中是接收不了的，尤其是在远程传输图像的场景中，图像占用的带宽过大，会对无线网络造成很大压力。实际应用中，图像在传输前往往会进行压缩处理，ROS也设计了压缩图像的消息类型——sensor_msgs/CompressedImage，该消息类型定义如下图所示。

这个消息类型相比原始图像的定义要简介不少，除了消息头外，只包含图像的压缩编码格式“format”和存储图像数据的“data”数组，图像压缩编码格式包含JPRG、PNG、BMP等，每种编码格式对数据的结构已经进行了详细定义，所以在消息类型定义中省去了很多不必要的信息。

2、三维点云数据

Kinect数据显示中，rviz订阅camera/depth_registered/points话题后，主界面即可显示三维点云数据，那么这种三维点云数据的消息类型是什么？可以使用如下命令查看：

$ rostopic info /camera/depth_registered/points

该消息类型对应rviz中可视化插件类型，使用以下命令查看该消息类型的具体结构：

$ rosmsg show sensor_msgs/PointCloud2

三维点云数据消息定义如下：

  • height：点云图像的纵向分辨率。
  • width：点云图像的横向分辨率。
  • fields：每个点的数据类型。
  • is_bigendian：数据的大小端存储模式。
  • point_step：单点的数据字节步长。
  • row_step：一列数据的字节步长。
  • data：点云数据的存储数组，总字节大小为 [公式] 。
  • is_dense：是否有无效点。

点云数据中每个像素点的三维坐标都是浮点数，而且包含图像数据，所以单帧数据量也很大。如果使用分布式网络的传输，在带宽有限的前提下，需要考虑能否满足数据的传输要求，或者针对数据进行压缩。

四、摄像头标定

摄像头是精密仪器，对光学器件的要求较高，由于摄像头内部与外部的一些原因，生成的物体图像往往会发生畸变，为了避免数据源造成的误差，需要针对摄像头的参数进行标定。ROS官方提供了用于双目和单目摄像头标定的功能包——camera_calibaration。

1、camera_calibaration功能包

首先使用以下命令安装摄像头标定功能包camera_calibaration：

$ sudo apt-get install ros-melodic-camera-calibration

标定需要用到下图所示的棋盘格图案的标定靶，请将该图案（在robot_vision/doc文件夹下）打印出来贴在平面硬纸板上备用。

2、启动标定程序

一切准备就绪后开始标定摄像头，首先启动USB摄像头：

$ roslaunch robot_vision usb_cam.launch

然后使用以下命令启动标定程序：

$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/usb_cam/image_raw camera:=/usb_cam

cameracalibrator.py程序需要输入以下几个参数：

  • size：标定棋盘格的内部角点个数，这里使用的棋盘一共有6行，每行有8个内部角点。
  • square：这个参数对应每个棋盘格的边长，单位是米。
  • image和camera：设置摄像头发布的图像话题。

根据使用的摄像头和标定靶棋盘格尺寸，相应修改以上参数，即可启动标定程序。

3、标定摄像头

标定程序启动成功后，将标定靶放置在摄像头视野范围内，应该可以看到如下图所示的图形界面。

在没有标定成功之前，右边的按钮都为灰色，不能点击。为了提高标定准确性，应该尽量让标定靶出现在摄像头视野范围内的各个区域，界面右上角的进度条会提示标定进度。

  • X：标定靶在摄像头视野的左右移动。
  • Y：标定靶在摄像头视野的上下移动。
  • Size：标定靶在摄像头视野的前后移动。
  • Skew：标定靶在摄像头视野的倾斜移动。

不断在视野中移动标定靶，直到“CALIBRATE”按钮变色，点击“CALIBRATE”按钮，标定程序开始自动计算摄像头的标定参数，这个过程需要等待一段时间，界面可能会变成灰色无响应状态，注意千万不要关闭。

参数计算完成后界面恢复，而且终端中会有标定结果的显示。

点击界面中的“SAVE”按钮，标定参数将被保存到默认文件夹下，并在终端看到该路径，如下图所示。

点击“COMMIT”按钮，提交数据并退出程序。

4、标定Kinect

除了一个RGB摄像头，Kinect还有一个红外摄像头，两个摄像头需要分别标定，方法与USB摄像头的标定相同，重新命名kinect_rgb_calibration，kinect_depth_calibration。

启动Kinect后，分别使用以下命令，按照上节流程即可完成标定。

$ roslaunch robot_vision freenect.launch
$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/camera/rgb/image_raw camera:=/camera/rgb
$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/camera/ir/image_raw camera:=/camera/ir

5、加载标定参数的配置文件

标定完成后，所有的YAML配置文件均自动生成，文件在~/.ros/camera_info文件夹下：

标定摄像头生成的配置文件是YAML格式的，可以在启动摄像头的launch文件中进行加载，例如加载摄像头标定文件的robot_vision/launch/usb_cam_with_calibration.launch：

<launch>

    <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
        <param name="video_device" value="/dev/video0" />
        <param name="image_width" value="640" />
        <param name="image_height" value="480" />
        <param name="pixel_format" value="yuyv" />
        <param name="camera_frame_id" value="usb_cam" />
        <param name="io_method" value="mmap"/>

        <param name="camera_info_url" type="string" value="file:///home/pan/.ros/camera_info/head_camera.yaml" />
    </node>

</launch>

Kinect标定文件的加载方法相同，分别设置RGB摄像头和红外深度摄像头的标定文件即可，详见robot_vision/launch/freenect_with_calibration.launch：

<launch>

    <!-- Launch the freenect driver -->
    <include file="$(find freenect_launch)/launch/freenect.launch">
        <arg name="publish_tf"                      value="true" /> 

        <!-- use device registration -->
        <arg name="depth_registration"              value="true" /> 

        <arg name="rgb_processing"                  value="true" />
        <arg name="ir_processing"                   value="false" />
        <arg name="depth_processing"                value="false" />
        <arg name="depth_registered_processing"     value="true" />
        <arg name="disparity_processing"            value="false" />
        <arg name="disparity_registered_processing" value="false" />
        <arg name="sw_registered_processing"        value="false" />
        <arg name="hw_registered_processing"        value="true" />

        <arg name="rgb_camera_info_url"
             value="file:///home/pan/.ros/camera_info/rgb_A00366902406104A.yaml" />
        <arg name="depth_camera_info_url"
             value="file:///home/pan/.ros/camera_info/depth_A00366902406104A.yaml" />
    </include>

</launch>

五、OpenCV库

OpenCV库是一个基于BSD许可发行的跨平台开源计算机库，可以运行在Linux、Windows和mac OS等操作系统上。OpenCV由一系列C函数和少量C++类构成，同时提供C++、Python、Ruby、Matlab等语言的接口，实现了图像处理和计算机视觉方面的很多通用算法。

1、安装OpenCV库

ROS中已经集成了OpenCV库和相关的接口功能包，使用如下命令即可安装：

$ sudo apt-get install ros-melodic-vision-opencv libopencv-dev python-opencv

2、在ROS中使用OpenCV

ROS开发者提供了与OpenCV的接口功能包——cv_bridge。如下图所示，开发者可以通过该功能包将ROS中的图像数据转换成OpenCV格式的图像，并且调用OpenCV库进行各种图像处理；或者将OpenCV处理过后的数据转换成ROS图像，通过话题发布，实现各节点之间的图像传输。

下面通过一个简单的例子了解如何使用cv_bridge完成ROS与Opencv之间的图像转换。在该例程中，一个ROS节点订阅摄像头驱动发布的图像消息，然后将其转换成OpenCV的图像格式进行显示，然后再将该OpenCV格式的图像转换成ROS图像消息进行发布并显示。

$ roslaunch robot_vision usb_cam.launch
$ rosrun robot_vision cv_bridge_test.py
$ rqt_image_view

运行效果如下图所示，图像左边是通过cv_bridge将ROS图像转换成OpenCV图像数据之后的显示效果，使用OpenCV库在图像的左上角绘制了一个红色的圆；图像右边是将OpenCV图像数据再次通过cv_bridge转换成ROS图像后的显示效果，左右两幅图像背景应该完全一致。

实现该例程的源代码robot_vision/scripts/cv_bridge_test.py的内容如下：

import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

class image_converter:
    def __init__(self):    
        # 创建cv_bridge，声明图像的发布者和订阅者
        self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.callback)

    def callback(self,data):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print e

        # 在opencv的显示窗口中绘制一个圆，作为标记
        (rows,cols,channels) = cv_image.shape
        if cols > 60 and rows > 60 :
            cv2.circle(cv_image, (60, 60), 30, (0,0,255), -1)

        # 显示Opencv格式的图像
        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)

        # 再将opencv格式额数据转换成ros image格式的数据发布
        try:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
        except CvBridgeError as e:
            print e

if __name__ == '__main__':
    try:
        # 初始化ros节点
        rospy.init_node("cv_bridge_test")
        rospy.loginfo("Starting cv_bridge_test node")
        image_converter()
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down cv_bridge_test node."
        cv2.destroyAllWindows()

分析以上例程代码关键部分：

import cv2
from cv_bridge import CvBridge, CvBridgeError

调用OpenCV，必须先导入OpenCV模块，另外还应导入cv_bridge所需要的一些模块。

self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.callback)

定义了一个Subscriber接收原始图像消息，再定义一个Publisher发布OpenCV处理后的图像消息，还要定义一个CvBridge的句柄，便于调用相关的转换接口。

try:
    cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
    print e

imgmsg_to_cv2()接口的功能就是将ROS图像消息转换成OpenCV图像数据，该接口有两个输入参数：第一个参数指向图像消息流，第二个参数用来定义转换的图像数据格式。

try:
    self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
    print e

cv2_to_imgmsg()接口的功能是将OpenCV格式的图像数据转换成ROS图像消息，该接口同样要求输入图像数据流和数据格式这两个参数。

六、人脸识别

人脸识别用于确定输入图像中人脸的位置（如果有）、大小和姿态，往往用于生物特征识别、视频监听、人机交互等应用中。2001年，Viola和Jones提出基于Haar特征的级联分类对象检测算法，并在2002年由Lienhart和Maydt进行改进，为快速、可靠的人脸检测应用提供了一种有效的方法。OpenCV已经集成了该算法的开源实现，利用大量样本的Haar特征进行分类器训练，然后调用训练好的瀑布级联分类器cascade进行模拟匹配。

如下图所示，OpenCV中的人脸识别算法首先将获取的图像进行灰度化转换，并进行边缘处理与噪声过滤；然后将图像缩小、直方图均衡化，同时将匹配分类器放大相同倍数，直到匹配分类器的大小大于检测图像，则返回匹配结果。匹配过程中，可以根据cascade分类器中的不同类型分别进行匹配，例如正脸和侧脸。

1、应用效果

我们只需要调用OpenCV相应接口就可以实现人脸识别功能，先不深究如何使用ROS和OpenCV实现人脸识别，我们先跑一下例程，看下效果。

使用如下命令启动摄像头，然后与运行face_detection.launch文件启动人脸识别功能：

$ roslaunch robot_vision usb_cam.launch
$ roslaunch robot_vision face_detector.launch
$ rqt_image_view

如下图所示，我的脸已经被识别出来了，识别到的人脸区域用绿色矩形框标识。

2、源码实现

现在让我们研究一下该例程的源码，即robot_vision/scripts/face_detector.py，主要分为以下三个部分。

（1）初始化部分

初始化部分主要完成ROS节点、图像、识别参数的设置。

    def __init__(self):
        rospy.on_shutdown(self.cleanup);

        # 创建cv_bridge
        self.bridge = CvBridge()
        self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)

        # 获取haar特征的级联表的XML文件，文件路径在launch文件中传入
        cascade_1 = rospy.get_param("~cascade_1", "")
        cascade_2 = rospy.get_param("~cascade_2", "")

        # 使用级联表初始化haar特征检测器
        self.cascade_1 = cv2.CascadeClassifier(cascade_1)
        self.cascade_2 = cv2.CascadeClassifier(cascade_2)

        # 设置级联表的参数，优化人脸识别，可以在launch文件中重新配置
        self.haar_scaleFactor  = rospy.get_param("~haar_scaleFactor", 1.2)
        self.haar_minNeighbors = rospy.get_param("~haar_minNeighbors", 2)
        self.haar_minSize      = rospy.get_param("~haar_minSize", 40)
        self.haar_maxSize      = rospy.get_param("~haar_maxSize", 60)
        self.color = (50, 255, 50)

        # 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射
        self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)

（2）ROS图像回调函数

例程节点收到摄像头发布的RGB图像数据后进入回调函数，将图像转换成OpenCV的数据格式，然后预处理之后开始调用人脸识别的功能函数，最后发布识别结果。

    def image_callback(self, data):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")     
            frame = np.array(cv_image, dtype=np.uint8)
        except CvBridgeError, e:
            print e

        # 创建灰度图像
        grey_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        # 创建平衡直方图，减少光线影响
        grey_image = cv2.equalizeHist(grey_image)

        # 尝试检测人脸
        faces_result = self.detect_face(grey_image)

        # 在opencv的窗口中框出所有人脸区域
        if len(faces_result)>0:
            for face in faces_result: 
                x, y, w, h = face
                cv2.rectangle(cv_image, (x, y), (x+w, y+h), self.color, 2)

        # 将识别后的图像转换成ROS消息并发布
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))

（3）人脸识别

人脸识别部分没有很多代码，直接调用OpenCV提供的人脸识别接口，与数据库中的人脸特征进行匹配。

    def detect_face(self, input_image):
        # 首先匹配正面人脸的模型
        if self.cascade_1:
            faces = self.cascade_1.detectMultiScale(input_image, 
                    self.haar_scaleFactor, 
                    self.haar_minNeighbors, 
                    cv2.CASCADE_SCALE_IMAGE, 
                    (self.haar_minSize, self.haar_maxSize))代码中youy
                                         
        # 如果正面人脸匹配失败，那么就尝试匹配侧面人脸的模型
        if len(faces) == 0 and self.cascade_2:
            faces = self.cascade_2.detectMultiScale(input_image, 
                    self.haar_scaleFactor, 
                    self.haar_minNeighbors, 
                    cv2.CASCADE_SCALE_IMAGE, 
                    (self.haar_minSize, self.haar_maxSize))
        
        return faces

代码中有一些参数和话题名需要在launch文件中设置，所以还需要编写一个运行例程的launch文件robot_vision/launch/face_detector.launch：

<launch>
    <node pkg="robot_vision" name="face_detector" type="face_detector.py" output="screen">
        <remap from="input_rgb_image" to="/usb_cam/image_raw" />
        <rosparam>
            haar_scaleFactor: 1.2
            haar_minNeighbors: 2
            haar_minSize: 40
            haar_maxSize: 60
        </rosparam>
        <param name="cascade_1" value="$(find robot_vision)/data/haar_detectors/haarcascade_frontalface_alt.xml" />
        <param name="cascade_2" value="$(find robot_vision)/data/haar_detectors/haarcascade_profileface.xml" />
    </node>
</launch>

七、物体跟踪

物体追踪和物体识别有些类似，同样使用特征点检测的方法，但侧重点并不相同。物体识别针对的物体可以是静态的或动态的，根据物体特征点建立的模型作为识别的数据依据；物体跟踪更强调对物体位置的准确定位，输入图像一般需要具有动态特性。

如下图所示，物体跟踪功能首先根据输入的图像流和选择跟踪的物体，采样物体在图像当前帧中的特征点；然后将当前帧和下一帧图像进行灰度值比较，估计出当前帧中跟踪物体的特征点在下一帧图像中的位置；再过滤位置不变的特征点，余下的点就是跟踪物体在第二帧图像中的特征点，其特征点集群即为跟踪物体的位置。该功能依然基于OpenCV提供的图像处理算法。

1、应用效果

使用以下命令启动摄像头，然后运行motion_detector.launch文件启动物体跟踪例程：

$ roslaunch robot_vision usb_cam.launch
$ roslaunch robot_vision motion_detector.launch
$ rqt_image_view

尽量选择纯色背景和色彩差异较大的测试物体。在画面中移动识别物体，即可看到矩形框标识出了运动物体的实时位置，可以针对实验环境调整识别区域、阈值等参数。

物体跟踪演示
Melody 的视频
 · 121 播放

2、源码实现

类似于人脸识别，物体跟踪的实现同样使用OpenCV提供的图像处理接口。该功能完整代码在robot_vision/scripts/motion_detector.py，主要有以下两个部分。

（1）初始化部分

初始化部分主要完成ROS节点、图像、识别参数设置，代码如下：

   def __init__(self):
        rospy.on_shutdown(self.cleanup);

        # 创建cv_bridge
        self.bridge = CvBridge()
        self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)

        # 设置参数：最小区域、阈值
        self.minArea   = rospy.get_param("~minArea",   500)
        self.threshold = rospy.get_param("~threshold", 25)

        self.firstFrame = None
        self.text = "Unoccupied"

        # 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射
        self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)

（2）图像处理部分

例程节点收到摄像头发布的RGB图像后，进入回调函数，将图像转换成OpenCV格式；完成图像预处理之后开始针对两帧图像进行比较，基于图像差异识别到运动的物体，最后标识识别结果并发布。

    def image_callback(self, data):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")     
            frame = np.array(cv_image, dtype=np.uint8)
        except CvBridgeError, e:
            print e

        # 创建灰度图像
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        gray = cv2.GaussianBlur(gray, (21, 21), 0)

        # 使用两帧图像做比较，检测移动物体的区域
        if self.firstFrame is None:
            self.firstFrame = gray
            return  
        frameDelta = cv2.absdiff(self.firstFrame, gray)
        thresh = cv2.threshold(frameDelta, self.threshold, 255, cv2.THRESH_BINARY)[1]

        thresh = cv2.dilate(thresh, None, iterations=2)
        binary, cnts, hierarchy= cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        for c in cnts:
            # 如果检测到的区域小于设置值，则忽略
            if cv2.contourArea(c) < self.minArea:
               continue 

            # 在输出画面上框出识别到的物体
            (x, y, w, h) = cv2.boundingRect(c)
            cv2.rectangle(frame, (x, y), (x + w, y + h), (50, 255, 50), 2)
            self.text = "Occupied"

        # 在输出画面上打当前状态和时间戳信息
        cv2.putText(frame, "Status: {}".format(self.text), (10, 20),
            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)

        # 将识别后的图像转换成ROS消息并发布
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))

代码中有一些参数和话题名需要在launch文件中进行设置，所以还需要编写一个运行节点的launch文件robot_vision/launch/motion_detector.launch：

<launch>
    <node pkg="robot_vision" name="motion_detector" type="motion_detector.py" output="screen">
        <remap from="input_rgb_image" to="/usb_cam/image_raw" />
        <rosparam>
            minArea: 500
            threshold: 25
        </rosparam>
    </node>
</launch>

八、二维码识别

生活中运用二维码的场合很多，例如商场购物、共享单车。ROS中提供了多种二维码识别的功能包，本节我们介绍其中一个功能包（ar_track_alvar）来介绍二维码的识别方法。

1、ar_track_alvar功能包

安装该功能包：

$ sudo apt-get install ros-melodic-ar-track-alvar

安装完成后，在ROS默认安装路径下(/opt/ros/melodic/share)找到它。打开该功能包的launch文件夹，可以看到很多个launch文件。这些都是针对PR2机器人使用的二维码识别例程，我们可以在其基础上修改。

2、创建二维码

ar_track_alvar功能包提供了二维码标签的生成功能，可以使用如下命令创建相应标号的二维码标签：

$ rosrun ar_track_alvar createMarker AR_ID

其中AR_ID可以是从0到65535之间任意数字的标号，例如：

$ rosrun ar_track_alvar createMarker 0

可以创建一个标号为0的二维码标签图片，命名为MarkerData_0.png，并放置在终端当前路径下。

createMarker工具还有很多参数可以进行配置，使用以下命令查看：

$ rosrun ar_track_alvar createMarker

从上图可以看出，createMarker不仅可以使用数字标号生成二维码标签，也可以使用字符串、文件名、网址等，还可以使用-s参数设置生成二维码的尺寸。

可以使用如下命令生成一系列二维码标签：

$ roscd robot_vision/config
$ rosrun ar_track_alvar createMarker -s 5 0
$ rosrun ar_track_alvar createMarker -s 5 1
$ rosrun ar_track_alvar createMarker -s 5 2

生成的二维码如下图所示，将他们打印出来贴在硬纸板上，以备使用。

3、摄像头识别二维码

ar_track_alvar功能包支持USB摄像头或RGB-D摄像头作为识别二维码的视觉传感器，分别应用于individualMarkersNoKinect和individualMarkers这两个不同的识别节点。

首先使用最常用的USB摄像头进行识别。复制ar_track_alvar功能包launch文件夹中的pr2_indiv_no_kinect.launch文件作为蓝本，针对使用的USB摄像头进行修改设置，重命名为robot_vision/launch/ar_track_camera.launch：

<launch>
    <node pkg="tf" type="static_transform_publisher" name="world_to_cam" 
          args="0 0 0.5 0 1.57 0 world usb_cam 10" />
        
    <arg name="marker_size" default="5" />
    <arg name="max_new_marker_error" default="0.08" />
    <arg name="max_track_error" default="0.2" />
    <arg name="cam_image_topic" default="/usb_cam/image_raw" />
    <arg name="cam_info_topic" default="/usb_cam/camera_info" />
    <arg name="output_frame" default="/usb_cam" />
        
    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
        <param name="marker_size"           type="double" value="$(arg marker_size)" />
        <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
        <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
        <param name="output_frame"          type="string" value="$(arg output_frame)" />

        <remap from="camera_image"  to="$(arg cam_image_topic)" />
        <remap from="camera_info"   to="$(arg cam_info_topic)" />
    </node>

    <!-- rviz view /-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find robot_vision)/config/ar_track_camera.rviz"/>
</launch>

launch文件主要进行了以下几点设置

  • 设置world与camera之间的坐标变换。
  • 设置individualMarkerNoKinect节点所需要的参数，主要是订阅图像数据的话题名，还有使用二维码的实际尺寸，单位是厘米。
  • 启动rviz界面，将识别结果可视化。

启动摄像头，并且launch文件启动二维码识别功能：

$ roslaunch robot_vision usb_cam_with_calibration.launch
$ roslaunch robot_vision ar_track_camera.launch
启动摄像头时，需要加载标定文件yaml，否则可能无法识别二维码。

运行成功后，可以在打开的rviz界面中看到摄像头信息，主界面中还有world和camera两个坐标系的显示。现将二维码标签放置到摄像头的视野范围内，很快就可以识别。很多个二维码被同时准确识别出来，图像中的二维码上会出现坐标轴，代表识别到的二维码姿态。ar_track
_alvar功能包不仅可以识别图像中的二维码，而且可以确定二维码的空间姿态，还可以计算二维码相对摄像头的空间位置。

其中，ar_pose_marker列出了所有识别到的二维码信息，包括ID号和二维码的位姿状态，可以使用“rostopic echo”打印该消息的数据。

获得这些数据后，我们就可以实现进一步的应用了，例如可以实现导航中的二维码定位、引导机器人跟随运动等功能。

4、Kinect识别二维码

同样可以使用Kinect等RGB-D摄像头识别二维码，对ar_track_alvar功能包的launch文件夹中的pr2_indiv.launch进行一些修改，重命名为robot_vision/launch/ar_track_kinect.launch：

<launch>

    <node pkg="tf" type="static_transform_publisher" name="world_to_cam" 
          args="0 0 0.5 0 1.57 0 world camera_rgb_optical_frame 10" />

    <arg name="marker_size" default="5.0" />
    <arg name="max_new_marker_error" default="0.08" />
    <arg name="max_track_error" default="0.2" />

    <arg name="cam_image_topic" default="/camera/depth_registered/points" />
    <arg name="cam_info_topic" default="/camera/rgb/camera_info" />
    <arg name="output_frame" default="/camera_rgb_optical_frame" />

    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
        <param name="marker_size" type="double" value="$(arg marker_size)" />
        <param name="max_new_marker_error" type="double" value="$(arg max_new_marker_error)" />
        <param name="max_track_error" type="double" value="$(arg max_track_error)" />
        <param name="output_frame" type="string" value="$(arg output_frame)" />

        <remap from="camera_image"  to="$(arg cam_image_topic)" />
        <remap from="camera_info"   to="$(arg cam_info_topic)" />
    </node>

    <!-- rviz view /-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find robot_vision)/config/ar_track_kinect.rviz"/>

</launch>

内容与ar_track_camera.launch基本一致，只是在调用二维码识别时改用individual-Markers节点。

接下来使用以下命令启动Kinect，并且运行ar_track_kinect.launch文件启动二维码识别功能：

$ roslaunch robot_vision freenect_with_calibration.launch
$ roslaunch robot_vision ar_track_kinect.launch

启动成功后，将二维码放置在Kinect视野范围内，可以在rviz中看到识别结果。

九、本章小节

机器人视觉是机器人应用中涉及最多的领域，ROS针对视觉数据定义了2D和3D类型的消息结构，基于这些数据，我们学习了以下内容：

  • 摄像头标定方法。
  • ROS中OpenCV的使用方法。
  • 基于ROS+OpenCV实现人脸识别、物体跟踪。
  • 使用摄像头和Kinect进行二维码识别、定位。