ROS 物体跟踪

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

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

类似于人脸识别，物体跟踪的实现同样使用OpenCV提供的图像处理接口。该应用实现的完
整代码是robot_vision/script/face_detector.py/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>

运行效果：

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

roslaunch robot_vision usb_cam.launch
roslaunch robot_vision motion_detector.launch

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