一、准备工作
1、开源库：
OpenCV,
二维图像处理和机器学习
OpenN I2 +OpenKinect(freenect),
深度传感器(MicrosoftKinect and Asus Xtion Pro)驱动和处理库
PCL.
点云库 处理三维点云数据
2维特征提取包 find_object_2d (ROS package)
https://github.com/introlab/find-object
http://wiki.ros.org/find_object_2d
补充参考：https://github.com/introlab/find-object
3D物体识别（https://github.com/wg-perception）
2D & 3D Object Detection 目标检测 算法和论文
直接安装
# ROS Kinetic:
$ sudo apt-get install ros-kinetic-find-object-2d
# ROS Jade:
$ sudo apt-get install ros-jade-find-object-2d
# ROS Indigo:
$ sudo apt-get install ros-indigo-find-object-2d
# ROS Hydro:
$ sudo apt-get install ros-hydro-find-object-2d
源码安装
$ cd ~/catkin_ws
$ git clone https://github.com/introlab/find-object.git src/find_object_2d
$ catkin_make
运行
$ roscore &
# Launch your preferred usb camera driver
$ rosrun uvc_camera uvc_camera_node &
$ rosrun find_object_2d find_object_2d image:=image_raw
2、图像分辨率：
160x120 (QQVGA), 320x240 (QVGA), 640x480 (VGA) ， 1280x960 (SXGA).
3、安装深度传感器驱动： ROS OpenNI and OpenKinect (freenect) Drivers sudo apt-get install ros-indigo-openni-* ros-indigo-openni2-* ros-indigo-freenect-* $ rospack profile
4、安装普通摄像头驱动 Webcam Driver
源码安装
usb_cam:
cd ~/catkin_ws/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git
cd ~/catkin_ws
catkin_make
rospack profile
libuvc_ros:
https://github.com/ros-drivers/libuvc_ros
uvc_cam:
cd cd ~/catkin_ws/src
git clone https://github.com/ericperko/uvc_cam.git
rosmake uvc_cam
5、测试图像传感器
对于 rgb-d传感器
使用 image_view 包 测试 http://wiki.ros.org/image_view
发布数据 在/camera/rgb/image_raw话题上
Microsoft Kinect:
$ roslaunch freenect_launch freenect-registered-xyzrgb.launch
Asus Xtion, Xtion Pro, or Primesense 1.08/1.09 cameras:
$ roslaunch openni2_launch openni2.launch depth_registration:=true
可视化查看 rgb图像
rosrun image_view image_view image:=/camera/rgb/image_raw
可视化查看 深度图像
$ rosrun image_view image_view image:=/camera/depth_registered/image_rect
对于Webcam Driver
发布消息
$ roslaunch rbx1_vision usb_cam.launch video_device:=/dev/video0
or
$ roslaunch rbx1_vision usb_cam.launch video_device:=/dev/video1
可视化查看
rosrun image_view image_view image:=/camera/rgb/image_raw
<launch>
<arg name="video_device" default="/dev/video0" /> 驱动名字
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" clear_params="true" output="screen">##节点信息
<remap from="usb_cam/image_raw" to="/camera/rgb/image_raw" /> 话题重定向
<remap from="usb_cam/camera_info" to="/camera/rgb/camera_info" />
<param name="video_device" value="$(arg video_device)" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="framerate" value="30" />
<param name="pixel_format" value="yuyv" /> 这里注意需要和驱动程序输出的图片格式一致 原先为mmp格式
<param name="contrast" value="32" /> 对比度
<param name="brightness" value="32" /> 亮度
<param name="saturation" value="32" /> 饱和度
<param name="autofocus" value="true" />
<param name="camera_frame_id" value="camera_link" />
</node>
</launch>
6、安装 ros 支持的opencv
$ sudo apt-get install ros-indigo-vision-opencv libopencv-dev \
python-opencv
$ rospack profile
7、安装 ros连接opencv 桥梁包 cv_bridge Packag 转换 ros图片格式 与 opencv图片格式
http://wiki.ros.org/cv_bridge
rbx1_vision/nodes/cv_bridge.demo.py 展示了怎样使用 cv_bridge
cv_bridge.demo.py
#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
import sys
import cv2
import cv2.cv as cv
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
class cvBridgeDemo():
def __init__(self):
self.node_name = "cv_bridge_demo"
rospy.init_node(self.node_name)
# 关闭
rospy.on_shutdown(self.cleanup)
# 创建 rgb图像 显示窗口
self.cv_window_name = self.node_name
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# 创建深度图像显示窗口
cv.NamedWindow("Depth Image", cv.CV_WINDOW_NORMAL)
cv.MoveWindow("Depth Image", 25, 350)
# 创建 ros 图 到 opencv图像转换 对象
self.bridge = CvBridge()
# 订阅 深度图像和rgb图像数据发布的话题 以及定义 回调函数
# the appropriate callbacks
self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback, queue_size=1)
# 登陆信息
rospy.loginfo("Waiting for image topics...")
rospy.wait_for_message("input_rgb_image", Image)
rospy.loginfo("Ready.")
# 收到rgb图像后的回调函数
def image_callback(self, ros_image):
# 转换图像格式到opencv格式
try:
frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
except CvBridgeError, e:
print e
# 转换成 numpy 图像数组格式
frame = np.array(frame, dtype=np.uint8)
# 简单处理图像数据 颜色 滤波 边缘检测等
display_image = self.process_image(frame)
# 显示图像
cv2.imshow(self.node_name, display_image)
# 检测键盘按键事件
self.keystroke = cv2.waitKey(5)
if self.keystroke != -1:
cc = chr(self.keystroke & 255).lower()
if cc == 'q':
# The user has press the q key, so exit
rospy.signal_shutdown("User hit q key to quit.")
# 收到深度图像后的回调函数
def depth_callback(self, ros_image):
# 转换图像格式到opencv格式
try:
# Convert the depth image using the default passthrough encoding
depth_image = self.bridge.imgmsg_to_cv2(ros_image, "passthrough")
except CvBridgeError, e:
print e
# 转换成 numpy 图像数组格式
depth_array = np.array(depth_image, dtype=np.float32)
# 深度图像 数据 正则化到 二值图像
cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
# 深度图像处理
depth_display_image = self.process_depth_image(depth_array)
# 现实结果
cv2.imshow("Depth Image", depth_display_image)
# rgb图像处理
def process_image(self, frame):
# 转化成灰度图像
grey = cv2.cvtColor(frame, cv.CV_BGR2GRAY)
# 均值滤波
grey = cv2.blur(grey, (7, 7))
# Canny 边缘检测
edges = cv2.Canny(grey, 15.0, 30.0)
return edges
# 深度图像处理
def process_depth_image(self, frame):
# 这里直接返回原图 可做其他处理
return frame
# 关闭节点 销毁所有 窗口
def cleanup(self):
print "Shutting down vision node."
cv2.destroyAllWindows()
# 主函数
def main(args):
try:
cvBridgeDemo()
rospy.spin()
except KeyboardInterrupt:
print "Shutting down vision node."
cv.DestroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
安装 照相机驱动程序 获取视频流 usb_cam
cd catkin_ws/src
git clone git://github.com/bosch-ros-pkg/usb_cam.git
catkin_make
source ~/catkin_ws/devel/setup.bash
使用rgb 摄像头获取的图像数据测试 上述节点功能 注意发布数据的话题 重映射到 上述节点指定的话题
<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"/>
<remap from="/usb_cam/image_raw" to="camera/image_raw"/>
</node>
</node>
<node name="cvDemo" pkg="vision_system" type="my_cv_bridge_demo.py" output="screen">
</node>
</launch>
鼠标 选取 感兴趣区域
#!/usr/bin/env python
# -*- coding:utf-8 -*-
"""
使用opencv2 跟踪 用户鼠标选择的 区域
"""
import rospy # ros系统操作
import sys # 系统程序 输入参数的获取等
import cv2 # 新版opencv2 api
import cv2.cv as cv # 老版 opencv api
from std_msgs.msg import String # ros系统 消息类型 来自 std_msgs.msg 标准消息类型
from sensor_msgs.msg import Image, RegionOfInterest, CameraInfo # ros 系统 消息类型 来自 sensor_msgs 传感器消息类型包
from cv_bridge import CvBridge, CvBridgeError # ros 系统 图像 格式 转换到 opencv图像格式 以及转换失败的错误信息处理
import time # 计时
import numpy as np # numpy 的数组
class ROS2OpenCV2(object):
def __init__(self, node_name):
self.node_name = node_name
rospy.init_node(node_name)
rospy.loginfo("启动节点 " + str(node_name))
# 关闭
rospy.on_shutdown(self.cleanup)
# 一些参数 可由 launch文件 修改的参数
self.show_text = rospy.get_param("~show_text", True)
self.show_features = rospy.get_param("~show_features", True)
self.show_boxes = rospy.get_param("~show_boxes", True)
self.flip_image = rospy.get_param("~flip_image", False)
self.feature_size = rospy.get_param("~feature_size", 10) # 点 圆形 的 半径
# 传感器消息类型 感兴趣区域 发布话题
self.ROI = RegionOfInterest()
self.roi_pub = rospy.Publisher("/roi", RegionOfInterest, queue_size=1)
# 初始化 全局变量
self.frame = None
self.frame_size = None
self.frame_width = None
self.frame_height = None
self.depth_image = None
self.marker_image = None
self.display_image = None
self.grey = None
self.prev_grey = None
self.selected_point = None
self.selection = None
self.drag_start = None
self.keystroke = None
self.detect_box = None # 检测的目标区域位置框
self.track_box = None # 跟踪的目标区域位置框
self.display_box = None
self.keep_marker_history = False
self.night_mode = False
self.auto_face_tracking = False
self.cps = 0 # 每秒 处理 次数 Cycles per second = number of processing loops per second.
self.cps_values = list()
self.cps_n_values = 20
self.busy = False
self.resize_window_width = 0 #窗口大小
self.resize_window_height = 0
self.face_tracking = False
# 创建 显示 窗口
self.cv_window_name = self.node_name
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
if self.resize_window_height > 0 and self.resize_window_width > 0:
cv.ResizeWindow(self.cv_window_name, self.resize_window_width, self.resize_window_height)
# 创建 ros 图 到 opencv图像转换 对象 bridge
self.bridge = CvBridge()
# 设置对应窗口 鼠标点击事件 回调函数
cv.SetMouseCallback (self.node_name, self.on_mouse_click, None)
# 订阅 深度图像和rgb图像数据发布的话题 以及定义 回调函数
self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback, queue_size=1)
# 鼠标点击事件回调函数
def on_mouse_click(self, event, x, y, flags, param):
# 允许用户用鼠标选者一个感兴趣区域 一个矩形框图 全局变量 selection 矩形框 <左上角点x 左上角点y 宽度 高度>
if self.frame is None:
return
# 鼠标按下
if event == cv.CV_EVENT_LBUTTONDOWN and not self.drag_start:
self.features = [] # 初始化感兴趣区域
self.track_box = None # 跟踪 框
self.detect_box = None# 检测 框
self.selected_point = (x, y)# 选择点
self.drag_start = (x, y) # 拖拽起点 后 开始拖拽
# 鼠标抬起
if event == cv.CV_EVENT_LBUTTONUP:
self.drag_start = None # 拖拽结束
self.classifier_initialized = False
self.detect_box = self.selection
# 鼠标拖拽
if self.drag_start:
xmin = max(0, min(x, self.drag_start[0]))# 起点 横坐标 为 鼠标选者区域的 横向 最小值 min(x, self.drag_start[0])
ymin = max(0, min(y, self.drag_start[1]))# 起点 纵坐标
xmax = min(self.frame_width, max(x, self.drag_start[0])) # 终点为最大值 鼠标选者区域的 横向 最大值 man(x, self.drag_start[0])
ymax = min(self.frame_height, max(y, self.drag_start[1]))
self.selection = (xmin, ymin, xmax - xmin, ymax - ymin) # 矩形区域 左上角 起点（x,y） 长 宽
def image_callback(self, data):
# 图像 header 时间 大小等信息
self.image_header = data.header
# 这次循环开始时间
start = time.time()
# 转换成opencv2图像格式
frame = self.convert_image(data)
# 翻转图像 翻转方向：1：水平翻转；0：垂直翻转；-1：水平垂直翻转
if self.flip_image:
frame = cv2.flip(frame, 0)
# 设置图像 尺寸
if self.frame_width is None:
self.frame_size = (frame.shape[1], frame.shape[0])# 列 为 宽度 行为高度
self.frame_width, self.frame_height = self.frame_size
# 创建可视化 marker image marker图像 可在rviz中查看
if self.marker_image is None:
self.marker_image = np.zeros_like(frame)
# 将祯图像 设置成全局变量
self.frame = frame.copy()
# 如果 不 记录历史图像 重置 可视化 marker image
if not self.keep_marker_history:
self.marker_image = np.zeros_like(self.marker_image)
# 处理图像 检测和跟踪 目标
processed_image = self.process_image(frame)
# 如果是 单通道 转化成 多通道 创建全局 处理后的图像
#if processed_image.channels == 1:
#cv.CvtColor(processed_image, self.processed_image, cv.CV_GRAY2BGR)
#else:
# 创建全局 处理后的图像
self.processed_image = processed_image.copy()
# 显示户选择的 矩形框
self.display_selection()
# Night mode: 仅显示 processed_image
if self.night_mode:
self.processed_image = np.zeros_like(self.processed_image)
# 混合 原图 marker image 和处理后的 图像
self.display_image = cv2.bitwise_or(self.processed_image, self.marker_image)
# 显示跟踪的 矩形框图
# cvRect (x,y,w,h) or a rotated Rect (center, size, angle).
if self.show_boxes:
if self.track_box is not None and self.is_rect_nonzero(self.track_box):
if len(self.track_box) == 4:
x,y,w,h = self.track_box
size = (w, h)
center = (x + w / 2, y + h / 2)
angle = 0
self.track_box = (center, size, angle)
else:
(center, size, angle) = self.track_box
# 对于人脸检测 的框 用垂直矩形框 更合适
if self.face_tracking:
pt1 = (int(center[0] - size[0] / 2), int(center[1] - size[1] / 2))
pt2 = (int(center[0] + size[0] / 2), int(center[1] + size[1] / 2))
cv2.rectangle(self.display_image, pt1, pt2, cv.RGB(50, 255, 50), self.feature_size, 8, 0)# 画 矩形
else:
# 其他 画 旋转的 矩形
vertices = np.int0(cv2.cv.BoxPoints(self.track_box))
cv2.drawContours(self.display_image, [vertices], 0, cv.RGB(50, 255, 50), self.feature_size)
# 显示检测 的 矩形 框图
elif self.detect_box is not None and self.is_rect_nonzero(self.detect_box):
(pt1_x, pt1_y, w, h) = self.detect_box
if self.show_boxes:
cv2.rectangle(self.display_image, (pt1_x, pt1_y), (pt1_x + w, pt1_y + h), cv.RGB(50, 255, 50), self.feature_size, 8, 0)
# 发布感兴趣区域到 感兴趣区域 消息类型 话题上
self.publish_roi()
# 计算处理时间 返回 处理速度 帧/s
end = time.time()
duration = end - start
fps = int(1.0 / duration)
self.cps_values.append(fps)
if len(self.cps_values) > self.cps_n_values:# 保持最新的 几个 处理速度 帧/s
self.cps_values.pop(0)
self.cps = int(sum(self.cps_values) / len(self.cps_values)) # 计算均值
# 显示 处理速度 帧/s CPS and image 以及图像分辨率
if self.show_text:
font_face = cv2.FONT_HERSHEY_SIMPLEX
font_scale = 0.5
""" 显示 的位置 """
if self.frame_size[0] >= 640:
vstart = 25
voffset = int(50 + self.frame_size[1] / 120.)
elif self.frame_size[0] == 320:
vstart = 15
voffset = int(35 + self.frame_size[1] / 120.)
else:
vstart = 10
voffset = int(20 + self.frame_size[1] / 120.)
cv2.putText(self.display_image, "CPS: " + str(self.cps), (10, vstart), font_face, font_scale, cv.RGB(255, 255, 0))
cv2.putText(self.display_image, "RES: " + str(self.frame_size[0]) + "X" + str(self.frame_size[1]), (10, voffset), font_face, font_scale, cv.RGB(255, 255, 0))
# 更新图像显示
cv2.imshow(self.node_name, self.display_image)
# 处理键盘 按键命令
self.keystroke = cv2.waitKey(5)
if self.keystroke is not None and self.keystroke != -1:
try:
cc = chr(self.keystroke & 255).lower()
if cc == 'n':
self.night_mode = not self.night_mode
elif cc == 'f':
self.show_features = not self.show_features
elif cc == 'b':
self.show_boxes = not self.show_boxes
elif cc == 't':
self.show_text = not self.show_text
elif cc == 'q':
# The has press the q key, so exit
rospy.signal_shutdown("User hit q key to quit.")
except:
pass
# 深度图像的 回调函数
def depth_callback(self, data):
# 转换图像格式
depth_image = self.convert_depth_image(data)
# 翻转图像 翻转方向：1：水平翻转；0：垂直翻转；-1：水平垂直翻转
if self.flip_image:
depth_image = cv2.flip(depth_image, 0)
# 处理
processed_depth_image = self.process_depth_image(depth_image)
# 全局变量
self.depth_image = depth_image.copy()
self.processed_depth_image = processed_depth_image.copy()
# 转换 rgb图像
def convert_image(self, ros_image):
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
cv_image = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
return np.array(cv_image, dtype=np.uint8)
except CvBridgeError, e:
print e
# 转换深度图像
def convert_depth_image(self, ros_image):
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
depth_image = self.bridge.imgmsg_to_cv2(ros_image, "passthrough")
depth_image = np.array(depth_image, dtype=np.float32)
return depth_image
except CvBridgeError, e:
print e
# 发布 感兴趣区域 的尺寸 位置 到/roi话题上
def publish_roi(self):
if not self.drag_start:
if self.track_box is not None:
roi_box = self.track_box
elif self.detect_box is not None:
roi_box = self.detect_box
else:
return
try:
roi_box = self.cvBox2D_to_cvRect(roi_box)# 转换成 矩形区域
except:
return
# 下限 为 0
roi_box[0] = max(0, roi_box[0])
roi_box[1] = max(0, roi_box[1])
try:
ROI = RegionOfInterest()
ROI.x_offset = int(roi_box[0])
ROI.y_offset = int(roi_box[1])
ROI.width = int(roi_box[2])
ROI.height = int(roi_box[3])
self.roi_pub.publish(ROI)
except:
rospy.loginfo("发布ROI失败")
# 处理图像 跟踪 感兴趣区域
def process_image(self, frame):
return frame
def process_depth_image(self, frame):
return frame
# 显示户选择的 矩形框
def display_selection(self):
# 显示 举行框
if self.drag_start and self.is_rect_nonzero(self.selection):# 矩形框
x,y,w,h = self.selection
cv2.rectangle(self.marker_image, (x, y), (x + w, y + h), (0, 255, 255), self.feature_size)
self.selected_point = None
# 显示 点 圆
elif not self.selected_point is None:# 点
x = self.selected_point[0]
y = self.selected_point[1]
cv2.circle(self.marker_image, (x, y), self.feature_size, (0, 255, 255), self.feature_size)
# 检测矩形框 是否 为空
def is_rect_nonzero(self, rect):
# 左上角点 宽度 高度 类型
try:
(_,_,w,h) = rect
return (w > 0) and (h > 0)
except:
try:
# 左上角点 大小 角度
((_,_),(w,h),a) = rect
return (w > 0) and (h > 0)
except:
return False
# 矩形款 格式转换 <左上角点 大小 角度> 到 <左上角点x 左上角点y 宽度 高度>
def cvBox2D_to_cvRect(self, roi):
try:
if len(roi) == 3:
(center, size, angle) = roi
pt1 = (int(center[0] - size[0] / 2), int(center[1] - size[1] / 2))
pt2 = (int(center[0] + size[0] / 2), int(center[1] + size[1] / 2))
rect = [pt1[0], pt1[1], pt2[0] - pt1[0], pt2[1] - pt1[1]] # <左上角点x 左上角点y 宽度 高度>
else:
rect = list(roi)
except:
return [0, 0, 0, 0]
return rect
# 矩形框 格式转换 <左上角点x 左上角点y 宽度 高度> 到 <左上角点 大小 角度>
def cvRect_to_cvBox2D(self, roi):
try:
if len(roi) == 3:
box2d = roi
else:
(p1_x, p1_y, width, height) = roi
center = (int(p1_x + width / 2), int(p1_y + height / 2))
size = (width, height)
angle = 0
box2d = (center, size, angle)
except:
return None
return list(box2d)
# 清理退出函数 关闭节点 销毁所有 窗口
def cleanup(self):
print "关闭了视觉信息处理节点。"
cv2.destroyAllWindows() # 关闭所有窗口
# 主函数
def main(args):
try:
node_name = "ros2opencv2"
ROS2OpenCV2(node_name)
rospy.spin()
except KeyboardInterrupt:
print "关闭了视觉信息处理节点。"
cv.DestroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
ros_object_analytics 单帧点云(欧氏距离聚类分割) + Yolo_v2/MobileNet_SSD 3D物体检测简介：
物体分析 Object Analytics (OA) 是一个ros包，
支持实时物体检测定位和跟踪(realtime object detection, localization and tracking.)
使用 RGB-D 相机输入,提供物体分析服务，为开发者开发更高级的机器人高级特性应用，
例如智能壁障(intelligent collision avoidance),和语义SLAM(semantic SLAM).
订阅:
RGB-D 相机发布的点云消息[sensor_msgs::PointClould2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html)
发布话题到:
物体检测 [object detection](https://github.com/intel/object_msgs),
物体跟踪 [object tracking](https://github.com/intel/ros_object_analytics/tree/master/object_analytics_msgs),
物体3d定位 [object localization](https://github.com/intel/ros_object_analytics/object_analytics_msgs)
2D 目标检测算法：
1. 基于 图形处理器(GPU) 运行的目标检测 ,
ros_opencl_caffe, Yolo v2 model and OpenCL Caffe framework
https://github.com/intel/ros_opencl_caffe
2. 基于 视觉处理器（VPU） 运行的目标检测 ,
ros_intel_movidius_ncs (devel branch),
with MobileNet_SSD model and Caffe framework.
https://github.com/Ewenwan/ros_intel_movidius_ncs
(Movidius神经计算棒,首个基于USB模式的深度学习推理工具和独立的人工智能（AI）加速器)
英特尔的子公司Movidius宣布推出Movidius Myriad X视觉处理器（VPU），
该处理器是一款低功耗SoC，主要用于基于视觉的设备的深度学习和AI算法加速，比如无人机、智能相机、VR/AR头盔。
就在不久前的上个月，Movidius还推出了基于Myriad 2芯片的神经计算棒Movidius Neural Compute Stick。
节点订阅的 传感器发布的话题
RGB图像 object_analytics/rgb (sensor_msgs::Image)
点云 object_analytics/pointcloud (sensor_msgs::PointCloud2)
节点发布的处理后得到的信息
定位信息(3d边框) object_analytics/localization (object_analytics_msgs::ObjectsInBoxes3D)
跟踪信息 object_analytics/tracking (object_analytics_msgs::TrackedObjects)
检测信息(2d边框)object_analytics/detection (object_msgs::ObjectsInBoxes)
object_analytics 节点分析
1. RGBD传感器预处理分割器 splitter
输入: /camera/depth_registered/points
输出: pointcloud 3d点
输出: rgb 2d图像
object_analytics_nodelet/splitter/SplitterNodelet
2. 点云分割处理器 segmenter
object_analytics_launch/launch/includes/segmenter.launch
输入: pointcloud 3d点
输出: segmentation 分割
object_analytics_nodelet/segmenter/SegmenterNodelet
object_analytics_nodelet/src/segmenter/segmenter_nodelet.cpp
订阅发布话题后
std::unique_ptr<Segmenter> impl_;
点云话题回调函数:
boost::shared_ptr<ObjectsInBoxes3D> msg = boost::make_shared<ObjectsInBoxes3D>();// 3d框
msg->header = points->header;
impl_->segment(points, msg);//检测
pub_.publish(msg); //发布检测消息
object_analytics_nodelet/src/segmenter/segmenter.cpp
a. 首先 ros点云消息转化成 pcl点云消息
const sensor_msgs::PointCloud2::ConstPtr& points；
PointCloudT::Ptr pointcloud(new PointCloudT);
fromROSMsg<PointT>(*points, pcl_cloud);
b. 执行分割 Segmenter::doSegment()
std::vector<PointIndices> cluster_indices;// 点云所属下标
PointCloudT::Ptr cloud_segment(new PointCloudT);// 分割点云
std::unique_ptr<AlgorithmProvider> provider_;
std::shared_ptr<Algorithm> seg = provider_->get();// 分割算法
seg->segment(cloud, cloud_segment, cluster_indices);// 执行分割
AlgorithmProvider -> virtual std::shared_ptr<Algorithm> get() = 0;
Algorithm::segment()
object_analytics_nodelet/src/segmenter/organized_multi_plane_segmenter.cpp
class OrganizedMultiPlaneSegmenter : public Algorithm
OrganizedMultiPlaneSegmenter 类集成 Algorithm类
分割算法 segment(){} 基于pcl算法
1. 提取点云法线 OrganizedMultiPlaneSegmenter::estimateNormal()
2. 分割平面 OrganizedMultiPlaneSegmenter::segmentPlanes() 平面系数模型分割平面
3. 去除平面后 分割物体 OrganizedMultiPlaneSegmenter::segmentObjects() 欧氏距离聚类分割
c. 生成消息 Segmenter::composeResult()
for (auto& obj : objects)
{
object_analytics_msgs::ObjectInBox3D oib3;
oib3.min = obj.getMin();
oib3.max = obj.getMax();
oib3.roi = obj.getRoi();
msg->objects_in_boxes.push_back(oib3);
}
3. 3d定位器 merger
输入: 2d检测分割结果 detection
输入: 3d检测分割结果 segmentation
输出: 3d定位结果 localization
object_analytics_nodelet/merger/MergerNodelet
2d物体 和 3d物体 关系 ======
遍历 每一个2d物体
遍历 每一个3d物体(3d物体点云反投影到像素平面上的2d框)
计算 2d物体边框 和3d物体投影2d边框的相似度 两边框的匹配相似度 match = IOU * distance / AvgSize
记录和 该2d边框最相似的 3d物体id
4. 目标跟踪器 tracker
输入: 2d图像 rgb input_rgb
输入: 2d检测分割结果 detection input_2d
输出: 跟踪结果 tracking output
参数: 跟踪帧队列长度: aging_th：default="30"；
参数: 跟踪置信度: probability_th" default="0.5"
object_analytics_nodelet/tracker/TrackingNodelet
object_analytics_visualization 可视化
5. 3d定位可视化 visualization3d localization
输入: 2d检测结果 "detection_topic" default="/object_analytics/detection"
输入: 3d检测结果 "localization_topic" default="/object_analytics/localization"
输入: 2d跟踪结果 "tracking_topic" default="/object_analytics/tracking"
输出: rviz可视化话题 "output_topic" default="localization_rviz"
6. 2d跟踪可视化 visualization2d
输入: 2d检测结果 "detection_topic" default="/object_analytics/detection"
输入: 2d跟踪结果 "tracking_topic" default="/object_analytics/tracking"
输入: 2d图像 "image_topic" default="/object_analytics/rgb"
输出: rviz可视化话题 ""output_topic" default="tracking_rviz"
目标检测接口
GPU yolo_v2 目标检测后端
opencl_caffe_launch/launch/includes/nodelet.launch
来源 ros_opencl_caffe
输入: 2d图像 /usb_cam/image_raw input_topic
输出: 2d检测分割结果 input_detection output_topic
参数文件: param_file default= find opencl_caffe_launch/launch/includes/default.yaml"
模型文件 net_config_path: "/opt/clCaffe/models/yolo/yolo416/yolo_fused_deploy.prototxt"
权重文件 weights_path: "/opt/clCaffe/models/yolo/yolo416/fused_yolo.caffemodel"
类别标签文件 labels_path: "/opt/clCaffe/data/yolo/voc.txt"
节点： opencl_caffe/opencl_caffe_nodelet
opencl_caffe/src/nodelet.cpp
Nodelet::onInit() ---> loadResources()
检测器 detector_.reset(new DetectorGpu());
载入配置文件 detector_->loadResources(net_config_path, weights_path, labels_path)
订阅话题回调函数 sub_ = getNodeHandle().subscribe("/usb_cam/image_raw", 1, &Nodelet::cbImage, this);
Nodelet::cbImage();
网络前向推理 detector_->runInference(image_msg, objects)
发布话题 pub_.publish(objects);
DetectorGpu 类
opencl_caffe/src/detector_gpu.cpp
网络初始化:
net.reset(new caffe::Net<Dtype>(net_cfg, caffe::TEST, caffe::Caffe::GetDefaultDevice()));
net->CopyTrainedLayersFrom(weights);
模式：
caffe::Caffe::set_mode(caffe::Caffe::GPU);
caffe::Caffe::SetDevice(0);
载入图像:
cv::cvtColor(cv_bridge::toCvShare(image_msg, "rgb8")->image, image, cv::COLOR_RGB2BGR);
initInputBlob(resizeImage(image), input_channels);
网络前传:
net->Forward();
获取网络结果:
caffe::Blob<Dtype>* result_blob = net->output_blobs()[0];
const Dtype* result = result_blob->cpu_data();
const int num_det = result_blob->height();
检测结果:
object_msgs::ObjectInBox object_in_box;
object_in_box.object.object_name = labels_list[classid];
object_in_box.object.probability = confidence;
object_in_box.roi.x_offset = left;
object_in_box.roi.y_offset = top;
object_in_box.roi.height = bot - top;
object_in_box.roi.width = right - left;
objects.objects_vector.push_back(object_in_box);
VPU mobileNetSSD 目标检测后端
movidius_ncs_launch/launch/includes/ncs_stream_detection.launch
来源 ros_intel_movidius_ncs
输入: 2d图像 input_rgb input_topic
输出: 2d检测分割结果 input_detection output_topic
参数:
模型类型 name="cnn_type" default="mobilenetssd"
模型配置文件 name="param_file" default="$(find movidius_ncs_launch)/config/mobilenetssd.yaml"
网络图配置文件 graph_file_path: "/opt/movidius/ncappzoo/caffe/SSD_MobileNet/graph"
类别文件voc21 category_file_path: "/opt/movidius/ncappzoo/data/ilsvrc12/voc21.txt"
网络输入尺寸 network_dimension: 300
通道均值 :
channel1_mean: 127.5
channel2_mean: 127.5
channel3_mean: 127.5
归一化:
scale: 0.007843
节点文件 movidius_ncs_stream/NCSNodelet movidius_ncs_stream/src/ncs_nodelet.cpp
输入话题 input_topic ： 2d图像 /camera/rgb/image_raw
输出话题 output_topic : 2d检测框结果 detected_objects
ncs_manager_handle_ = std::make_shared<movidius_ncs_lib::NCSManager>();
movidius_ncs_lib::NCSManager 来自 movidius_ncs_lib/src/ncs_manager.cpp
NCSImpl::init(){ }
订阅 rgb图像的回调函数 cbDetect()
sub_ = it->subscribe("/camera/rgb/image_raw", 1, &NCSImpl::cbDetect, this);
cbDetect(){ }
1. 从话题复制图像
cv::Mat camera_data = cv_bridge::toCvCopy(image_msg, "bgr8")->image;
2. 提取检测结果回调函数
FUNP_D detection_result_callback = boost::bind(&NCSImpl::cbGetDetectionResult, this, _1, _2);
3. 进行目标检测
ncs_manager_handle_->detectStream(camera_data, detection_result_callback, image_msg);
NCSManager::detectStream(){}
得到检测结果 : movidius_ncs_lib::DetectionResultPtr result;
检测结果:
for (auto item : result->items_in_boxes)
object_msgs::ObjectInBox obj;
obj.object.object_name = item.item.category;
obj.object.probability = item.item.probability;
obj.roi.x_offset = item.bbox.x;
obj.roi.y_offset = item.bbox.y;
obj.roi.width = item.bbox.width;
obj.roi.height = item.bbox.height;
objs_in_boxes.objects_vector.push_back(obj);
发布检测结果:
objs_in_boxes.header = header;
objs_in_boxes.inference_time_ms = result->time_taken;
pub_.publish(objs_in_boxes);
扫一扫
4050
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
