目录
一、 RealsenseD435/D455实物 - YOLO - detect
二、ROS - RealsenseD435/D455 - gazebo仿真
三、ROS - RealsenseD435/D455 如何保存rgb与depth图
四、ROS - RealsenseD435仿真 - YOLO - detect
最终可以实现：在gazebo仿真环境中，realsenseD435相机能够检测出画面中的目标物体并且返回其(x,y,h)
一、 RealsenseD435/D455实物 - YOLO - detect
参考
realsense-D455-YOLOV5开源资料：
使用
win/ubuntu，装环境，插入设备，运行python realsensedetect.py，可以检测出coco数据集中的图像坐标与深度，如果想打印某目标的（x,y,h）：
label = '%s %.2f%s' % (names[int(cls)], np.mean(distance_list), 'm')
plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=3)
c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3]))
####上面三行方便定位代码位置，下面两行是需要添加的
if names[int(cls)]=='mouse': #以鼠标为例
print('xyh',mid_pos[0],mid_pos[1],np.mean(distance_list))二、ROS - RealsenseD435/D455实物
参考
GitHub - IntelRealSense/realsense-ros at ros1-legacy
1、如果遇到这个错误：
(messenger-libusb.cpp:42) control_transfer returned error, index: 300, error
检查usb线是否可用，相机是否连接正常
2、如果遇到这个错误：
[camera/realsense2_camera_manager-1] process has died [pid 11552, exit code 127, cmd /opt/ros/melodic/lib/nodelet/nodelet manager __name:=realsense2_camera_manager __log:=/home/nico/.ros/log/8a4aa5aa-f974-11ed-ac63-a333cc6ddd13/camera-realsense2_camera_manager-1.log].
ddynamic重复安装，卸载：
sudo apt-get remove ros-melodic-ddynamic-reconfigure三、ROS - RealsenseD435/D455仿真 - gazebo
参考
源码：
IntelRealSense / realsense-ros
issaiass / realsense2_description，其中提供了支持gazebo的插件
文档
https://dev.intelrealsense.com/docs/ros1-wrapper使用
运行：
roslaunch realsense2_description view_d435_model_rviz_gazebo.launch可以看到topic：
/camera/color/image_raw
/camera/depth/image_raw
四、ROS - RealsenseD435/D455 如何保存rgb与depth图
解释
/camera/color/image_raw 比较容易保存
/camera/depth/image_raw容易出问题，首先应该先确定编码方式
rostopic echo /camera/depth/image_raw/encodingbridge.imgmsg_to_cv2()函数可以将图像的ros话题转化从cv支持的格式，但容易保存成很黑的图片，参考了这篇回答
代码
#!/usr/bin/env python3
# coding:utf-8
import sys
import rospy
import numpy as np
from sensor_msgs.msg import Image
from sensor_msgs.msg import JointState
from cv_bridge import CvBridge, CvBridgeError
import cv2
class Dataset_Process:
def __init__(self):
rospy.init_node('img_process_node', anonymous=True)
self.bridge = CvBridge()
rospy.Subscriber('/camera/color/image_raw', Image, self.image_raw_callback)
rospy.Subscriber("/camera/depth/image_raw",Image, self.image_depth_callback)
def image_raw_callback(self,data):
self.color_image = data
self.color_image_cv = self.bridge.imgmsg_to_cv2(data, "bgr8")
def image_depth_callback(self,data):
self.depth_image = data
self.depth_image_cv = self.bridge.imgmsg_to_cv2(data, "16UC1")#32FC1
self.depth_array = np.array(self.depth_image_cv, dtype=np.float32)
self.depth_array[np.isnan(self.depth_array)] = 0
cv2.normalize(self.depth_array, self.depth_array, 0, 1, cv2.NORM_MINMAX)
def saveimg(self,imgsaving,filename):
filestr="/home/lidia/catkin_ws/src/offboard_node/images/"+filename+".png"
cv2.imwrite(filestr,imgsaving)
if __name__ == '__main__':
data_sample = Dataset_Process()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rate.sleep()
try:
time_now_secs = int(data_sample.color_image.header.stamp.secs)
time_now_nsecs = int(data_sample.color_image.header.stamp.nsecs)
rgb_file_name="rgb/img"+str(time_now_secs)+"_"+str(time_now_nsecs)
depth_file_name="depth/img"+str(time_now_secs)+"_"+str(time_now_nsecs)
data_sample.saveimg(data_sample.color_image_cv,rgb_file_name)
data_sample.saveimg(data_sample.depth_image_cv,depth_file_name)
print("saving..")
except:
print("try again")
rospy.spin()
四、ROS - RealsenseD435仿真 - YOLO - detect
结合前三部分的内容，可以将第一部分中的realsensedetect.py修改成如下代码，以实现：
在gazebo仿真环境中，realsenseD435相机能够检测出画面中的目标物体并且返回其(x,y,h)
代码
import argparse
import os
import shutil
import time
from pathlib import Path
import cv2
import torch
import torch.backends.cudnn as cudnn
from numpy import random
import numpy as np ##
import pyrealsense2 as rs ##
from models.experimental import attempt_load
from utils.general import (
check_img_size, non_max_suppression, apply_classifier, scale_coords,
xyxy2xywh, plot_one_box, strip_optimizer, set_logging)
from utils.torch_utils import select_device, load_classifier, time_synchronized
from utils.datasets import letterbox
import sys
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
class Dataset_Process:
def __init__(self):
rospy.init_node('img_process_node', anonymous=True)
self.bridge = CvBridge()
rospy.Subscriber('/camera/color/image_raw', Image, self.image_raw_callback)
rospy.Subscriber("/camera/depth/image_raw",Image, self.image_depth_callback)
self.detect()
# rospy.Subscriber("/joint_states",JointState,self.joint_state_callback)
def image_raw_callback(self,data):
self.color_image = data
self.color_image_cv = self.bridge.imgmsg_to_cv2(data, "bgr8")
# cv2.imshow("frame" , cv_img)
# cv2.waitKey(1)
# filestr="/home/lidia/catkin_ws/src/offboard_node/images/rgb/img"+str(data.header.stamp.secs)+"_"+str(data.header.stamp.nsecs)+".png"
# cv2.imwrite(filestr,cv_img)
def image_depth_callback(self,data):
self.depth_image = data
self.depth_image_cv = self.bridge.imgmsg_to_cv2(data, "16UC1")# 32FC1
self.depth_array = np.array(self.depth_image_cv, dtype=np.float32)
self.depth_array[np.isnan(self.depth_array)]=0
cv2.normalize(self.depth_array,self.depth_array,0,1,cv2.NORM_MINMAX)
def detect(self,save_img=False):
out, source, weights, view_img, save_txt, imgsz = \
opt.save_dir, opt.source, opt.weights, opt.view_img, opt.save_txt, opt.img_size
webcam = source == '0' or source.startswith(('rtsp://', 'rtmp://', 'http://')) or source.endswith('.txt')
# Initialize
set_logging()
device = select_device(opt.device)
if os.path.exists(out): # output dir
shutil.rmtree(out) # delete dir
os.makedirs(out) # make new dir
half = device.type != 'cpu' # half precision only supported on CUDA
# Load model
model = attempt_load(weights, map_location=device) # load FP32 model,载入模型
imgsz = check_img_size(imgsz, s=model.stride.max()) # check img_size
if half:
model.half() # to FP16
# Set Dataloader
vid_path, vid_writer = None, None
view_img = True
cudnn.benchmark = True # set True to speed up constant image size inference
#dataset = LoadStreams(source, img_size=imgsz)
# Get names and colors
names = model.module.names if hasattr(model, 'module') else model.names
colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))]
# Run inference
t0 = time.time()
img = torch.zeros((1, 3, imgsz, imgsz), device=device) # init img
_ = model(img.half() if half else img) if device.type != 'cpu' else None # run once
# pipeline = rs.pipeline()
# 创建 config 对象：
config = rs.config()
# config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 60)
# Start streaming
# pipeline.start(config)
align_to_color = rs.align(rs.stream.color)
while True:
start = time.time()
# Wait for a coherent pair of frames（一对连贯的帧）: depth and color
# frames = pipeline.wait_for_frames()
# frames = align_to_color.process(frames)
# depth_frame = frames.get_depth_frame() # 深度
# color_frame = frames.get_color_frame() # rgb
# print('depth_frame',type(depth_frame)) # <class 'pyrealsense2.pyrealsense2.depth_frame'>
# print('color_frame',type(color_frame)) # <class 'pyrealsense2.pyrealsense2.video_frame'>
# color_image = np.asanyarray(color_frame.get_data())
# depth_image = np.asanyarray(depth_frame.get_data())
# mask = np.zeros([color_image.shape[0], color_image.shape[1]], dtype=np.uint8)
# mask[0:480, 320:640] = 255
sources = [source]
imgs = [None]
path = sources
# imgs[0] = color_image # 检测是基于rgb图检测的
# im0s = imgs.copy()
# img = [letterbox(x, new_shape=imgsz)[0] for x in im0s]
# img = np.stack(img, 0)
# img = img[:, :, :, ::-1].transpose(0, 3, 1, 2) # BGR to RGB, to 3x416x416, uint8 to float32
# img = np.ascontiguousarray(img, dtype=np.float16 if half else np.float32)
# img /= 255.0 # 0 - 255 to 0.0 - 1.0
depth_image = self.depth_array
imgs[0] = self.color_image_cv
im0s = imgs.copy()
img = [letterbox(x, new_shape=imgsz)[0] for x in im0s]
# img = [letterbox(x, new_shape=imgsz)[0] for x in img]
img = np.stack(img, 0)
# print(img.shape)
img = img.transpose(0, 3, 1, 2) # BGR to RGB, to 3x416x416, uint8 to float32
img = np.ascontiguousarray(img, dtype=np.float16 if half else np.float32)
img /=255.0
# Get detections
img = torch.from_numpy(img).to(device) # 转化成了torch的格式传入model
if img.ndimension() == 3:
img = img.unsqueeze(0)
t1 = time_synchronized()
pred = model(img, augment=opt.augment)[0]
# Apply NMS
pred = non_max_suppression(pred, opt.conf_thres, opt.iou_thres, classes=opt.classes, agnostic=opt.agnostic_nms)
t2 = time_synchronized()
for i, det in enumerate(pred): # detections per image
p, s, im0 = path[i], '%g: ' % i, im0s[i].copy()
s += '%gx%g ' % img.shape[2:] # print string
gn = torch.tensor(im0.shape)[[1, 0, 1, 0]] # normalization gain whwh
if det is not None and len(det):
# Rescale boxes from img_size to im0 size
det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round()
# Print results
for c in det[:, -1].unique():
n = (det[:, -1] == c).sum() # detections per class
s += '%g %ss, ' % (n, names[int(c)]) # add to string，存放检测到了几个物体
# Write results
for *xyxy, conf, cls in reversed(det):
xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist() # normalized xywh
line = (cls, conf, *xywh) if opt.save_conf else (cls, *xywh) # label format
distance_list = []
mid_pos = [int((int(xyxy[0]) + int(xyxy[2])) / 2), int((int(xyxy[1]) + int(xyxy[3])) / 2)] # 确定索引深度的中心像素位置左上角和右下角相加在/2
min_val = min(abs(int(xyxy[2]) - int(xyxy[0])), abs(int(xyxy[3]) - int(xyxy[1]))) # 确定深度搜索范围
# print(box,)
randnum = 40
for i in range(randnum):
bias = random.randint(-min_val // 4, min_val // 4)
dist = depth_image[int(mid_pos[0] + bias), int(mid_pos[1] + bias)]###照片的xy位置
# print(max())
# print(int(mid_pos[1] + bias), int(mid_pos[0] + bias))
if dist:
distance_list.append(dist)
distance_list = np.array(distance_list)
distance_list = np.sort(distance_list)[randnum // 2 - randnum // 4:randnum // 2 + randnum // 4] # 冒泡排序+中值滤波
label = '%s %.2f%s' % (names[int(cls)], np.mean(distance_list), 'm')#label中存放着标签的名字和距离
plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=3)
c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3]))
# print(c1, '*******',c2 )
if names[int(cls)]=='airplane':
print('xyh',mid_pos[0],mid_pos[1],np.mean(distance_list))#如果是鼠标就返回xyh
# print(label,type(label))
# Print time (inference + NMS)
print('%sDone. (%.3fs)' % (s, t2 - t1))
# Stream results
if view_img:
cv2.imshow(p, im0)
if cv2.waitKey(1) == ord('q'): # q to quit
raise StopIteration
# print('Done. (%.3fs)' % (time.time() - t0))
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--weights', nargs='+', type=str, default='yolov5l.pt', help='model.pt path(s)')#在第一次执行的时候，会下载yolov5m.pt，是一个训练好的模型
parser.add_argument('--source', type=str, default='inference/images', help='source') # file/folder, 0 for webcam
parser.add_argument('--img-size', type=int, default=640, help='inference size (pixels)')
parser.add_argument('--conf-thres', type=float, default=0.25, help='object confidence threshold')
parser.add_argument('--iou-thres', type=float, default=0.45, help='IOU threshold for NMS')
parser.add_argument('--device', default='', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
parser.add_argument('--view-img', action='store_true', help='display results')
parser.add_argument('--save-txt', action='store_true', help='save results to *.txt')
parser.add_argument('--save-conf', action='store_true', help='save confidences in --save-txt labels')
parser.add_argument('--save-dir', type=str, default='inference/output', help='directory to save results')
parser.add_argument('--classes', nargs='+', type=int, help='filter by class: --class 0, or --class 0 2 3')
parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS')
parser.add_argument('--augment', action='store_true', help='augmented inference')
parser.add_argument('--update', action='store_true', help='update all models')
opt = parser.parse_args()
#print('***************',opt)
with torch.no_grad(): # 一个上下文管理器，被该语句wrap起来的部分将不会track梯度
data_sample = Dataset_Process()
8067
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
