上一節中我們實現了人臉的檢測,當有人臉出現攝像頭前面時,圖像窗體中會用矩形顯示人臉的位置,接下來我們需要實現特徵值獲取,及特徵值跟隨。本文針對Opencv3移植了ROS By Example Volume 1中的示例代碼,所有代碼均在opencv3環境完成測試,可以正常運行。
1.特徵值獲取
1.1 源代碼
源文件good_features.py請見github
在Opencv中已經提供了特徵值獲取的方法goodFeaturesToTrack,我們只需要在程序中設定ROI,然後調用goodFeaturesToTrack就可以獲取該ROI內的特徵點
源代碼中GoodFeatures繼承自ROS2OpenCV3,並且重寫了process_image函數,進行處理,並調用get_keypoints函數,在get_keypoints函數中調用goodFeaturesToTrack獲取detect_box內的特徵值。
#!/usr/bin/env python
import roslib
import rospy
import cv2
from ros2opencv3 import ROS2OpenCV3
import numpy as np
class GoodFeatures(ROS2OpenCV3):
def __init__(self, node_name):
super(GoodFeatures, self).__init__(node_name)
# Do we show text on the display?
self.show_text = rospy.get_param("~show_text", True)
# How big should the feature points be (in pixels)?
self.feature_size = rospy.get_param("~feature_size", 1)
# Good features parameters
self.gf_maxCorners = rospy.get_param("~gf_maxCorners", 200)
self.gf_qualityLevel = rospy.get_param("~gf_qualityLevel", 0.02)
self.gf_minDistance = rospy.get_param("~gf_minDistance", 7)
self.gf_blockSize = rospy.get_param("~gf_blockSize", 10)
self.gf_useHarrisDetector = rospy.get_param("~gf_useHarrisDetector", True)
self.gf_k = rospy.get_param("~gf_k", 0.04)
# Store all parameters together for passing to the detector
self.gf_params = dict(maxCorners = self.gf_maxCorners,
qualityLevel = self.gf_qualityLevel,
minDistance = self.gf_minDistance,
blockSize = self.gf_blockSize,
useHarrisDetector = self.gf_useHarrisDetector,
k = self.gf_k)
# Initialize key variables
self.keypoints = list()
self.detect_box = None
self.mask = None
def process_image(self, cv_image):
# If the user has not selected a region, just return the image
if not self.detect_box:
return cv_image
# Create a greyscale version of the image
grey = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# Equalize the histogram to reduce lighting effects
grey = cv2.equalizeHist(grey)
# Get the good feature keypoints in the selected region
keypoints = self.get_keypoints(grey, self.detect_box)
# If we have points, display them
if keypoints is not None and len(keypoints) > 0:
for x, y in keypoints:
cv2.circle(self.marker_image, (x, y), self.feature_size, (0, 255, 0, 0), cv2.FILLED, 8, 0)
# Process any special keyboard commands
if 32 <= self.keystroke and self.keystroke < 128:
cc = chr(self.keystroke).lower()
if cc == 'c':
# Clear the current keypoints
keypoints = list()
self.detect_box = None
return cv_image
def get_keypoints(self, input_image, detect_box):
# Initialize the mask with all black pixels
self.mask = np.zeros_like(input_image)
# Get the coordinates and dimensions of the detect_box
try:
x, y, w, h = detect_box
except:
return None
# Set the selected rectangle within the mask to white
self.mask[y:y+h, x:x+w] = 255
# Compute the good feature keypoints within the selected region
keypoints = list()
kp = cv2.goodFeaturesToTrack(input_image, mask = self.mask, **self.gf_params)
if kp is not None and len(kp) > 0:
for x, y in np.float32(kp).reshape(-1, 2):
keypoints.append((x, y))
return keypoints
if __name__ == '__main__':
try:
node_name = "good_features"
GoodFeatures(node_name)
rospy.spin()
except KeyboardInterrupt:
print "Shutting down the Good Features node."
cv2.destroyAllWindows()
1.2.launch文件
<launch>
<node pkg="diego_vision" name="good_features" type="good_features.py" output="screen">
<remap from="input_rgb_image" to="/camera/rgb/image_color" />
<rosparam>
gf_maxCorners: 200
gf_qualityLevel: 0.02
gf_minDistance: 7
gf_blockSize: 10
gf_useHarrisDetector: False
gf_k: 0.04
feature_size: 1
show_text: True
</rosparam>
</node>
</launch>
2.特徵值跟隨
1.1源代碼
源文件lk_tracker.py請見github
在Opencv中提供了方法calcOpticalFlowPyrLK提供在視頻中連續幀特徵點的匹配,其使用的算法是Lucas–Kanade method,有興趣的同學去深入瞭解算法。
源代碼中LKTracker繼承自GoodFeatures,並且重寫了process_image函數,進行處理,並調用track_keypoints函數,在track_keypoints函數中調用calcOpticalFlowPyrLK方法進行特徵點跟蹤。
#!/usr/bin/env python
import roslib
import rospy
import cv2
import numpy as np
from good_features import GoodFeatures
class LKTracker(GoodFeatures):
def __init__(self, node_name):
super(LKTracker, self).__init__(node_name)
self.show_text = rospy.get_param("~show_text", True)
self.feature_size = rospy.get_param("~feature_size", 1)
# LK parameters
self.lk_winSize = rospy.get_param("~lk_winSize", (10, 10))
self.lk_maxLevel = rospy.get_param("~lk_maxLevel", 2)
self.lk_criteria = rospy.get_param("~lk_criteria", (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 20, 0.01))
self.lk_params = dict( winSize = self.lk_winSize,
maxLevel = self.lk_maxLevel,
criteria = self.lk_criteria)
self.detect_interval = 1
self.keypoints = None
self.detect_box = None
self.track_box = None
self.mask = None
self.grey = None
self.prev_grey = None
def process_image(self, cv_image):
# If we don't yet have a detection box (drawn by the user
# with the mouse), keep waiting
if self.detect_box is None:
return cv_image
# Create a greyscale version of the image
self.grey = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# Equalize the grey histogram to minimize lighting effects
self.grey = cv2.equalizeHist(self.grey)
# If we haven't yet started tracking, set the track box to the
# detect box and extract the keypoints within it
if self.track_box is None or not self.is_rect_nonzero(self.track_box):
self.track_box = self.detect_box
self.keypoints = self.get_keypoints(self.grey, self.track_box)
else:
if self.prev_grey is None:
self.prev_grey = self.grey
# Now that have keypoints, track them to the next frame
# using optical flow
self.track_box = self.track_keypoints(self.grey, self.prev_grey)
# Process any special keyboard commands for this module
if 32 <= self.keystroke and self.keystroke < 128:
cc = chr(self.keystroke).lower()
if cc == 'c':
# Clear the current keypoints
self.keypoints = None
self.track_box = None
self.detect_box = None
self.prev_grey = self.grey
return cv_image
def track_keypoints(self, grey, prev_grey):
# We are tracking points between the previous frame and the
# current frame
img0, img1 = prev_grey, grey
# Reshape the current keypoints into a numpy array required
# by calcOpticalFlowPyrLK()
p0 = np.float32([p for p in self.keypoints]).reshape(-1, 1, 2)
# Calculate the optical flow from the previous frame to the current frame
p1, st, err = cv2.calcOpticalFlowPyrLK(img0, img1, p0, None, **self.lk_params)
# Do the reverse calculation: from the current frame to the previous frame
try:
p0r, st, err = cv2.calcOpticalFlowPyrLK(img1, img0, p1, None, **self.lk_params)
# Compute the distance between corresponding points in the two flows
d = abs(p0-p0r).reshape(-1, 2).max(-1)
# If the distance between pairs of points is < 1 pixel, set
# a value in the "good" array to True, otherwise False
good = d < 1
# Initialize a list to hold new keypoints
new_keypoints = list()
# Cycle through all current and new keypoints and only keep
# those that satisfy the "good" condition above
for (x, y), good_flag in zip(p1.reshape(-1, 2), good):
if not good_flag:
continue
new_keypoints.append((x, y))
# Draw the keypoint on the image
cv2.circle(self.marker_image, (x, y), self.feature_size, (0, 255, 0, 0), cv2.FILLED, 8, 0)
# Set the global keypoint list to the new list
self.keypoints = new_keypoints
# If we have enough points, find the best fit ellipse around them
if len(self.keypoints) > 6:
keypoints_array = np.float32([p for p in self.keypoints]).reshape(-1, 1, 2)
track_box = cv2.fitEllipse(keypoints_array)
else:
# Otherwise, find the best fitting rectangle
track_box = cv2.boundingRect(keypoints_matrix)
except:
track_box = None
return track_box
if __name__ == '__main__':
try:
node_name = "lk_tracker"
LKTracker(node_name)
rospy.spin()
except KeyboardInterrupt:
print "Shutting down LK Tracking node."
cv2.destroyAllWindows()
2.2 launch文件
<launch>
<node pkg="diego_vision" name="lk_tracker" type="lk_tracker.py" output="screen">
<remap from="input_rgb_image" to="/camera/rgb/image_color" />
<rosparam>
show_text: True
gf_maxCorners: 200
gf_qualityLevel: 0.02
gf_minDistance: 7
gf_blockSize: 10
gf_useHarrisDetector: False
gf_k: 0.04
feature_size: 1
</rosparam>
</node>
</launch>
3.測試
首先啓動運行xtion攝像頭
roslaunch diego_vision openni_node.launch
啓動lk_tracker
roslaunch diego_vision lk_tracker.launch
啓動後會出現視頻窗體,用鼠標選擇ROI,然後就會看到在ROI中檢測到的特徵點,這時候我們如果移動ROI內的物體,就會看到矩形框始終會定位到開始選擇的物體,跟隨物體的移動而移動。
下圖是本人測試的結果:ROI選擇在手部位置,當我移動手的位置時,矩形框就會跟隨移動。