位置:/home/znkz/catkin_ws/src/rbx1/rbx1_vision/nodes/image_acq.py
1、订阅话题获得彩色图片和深度图片
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback, queue_size=1) self.depth_sub = rospy.Subscriber("/camera/depth_registered/image_raw", Image, self.depth_callback, queue_size=1)彩色图:
深度图:
2、深度图——去除安全距离——开运算——提取轮廓——凸包
# -*- coding: utf-8 -*- """ Created on Tur Jun 5 12:15 2018 @author: gzh """ from numpy import * import numpy as np import cv2 from matplotlib import pyplot as plt from PIL import Image import matplotlib import cv2.cv as cv global nr global nc def mask_depth(image,th,throld = 1000): global nr global nc # nr = image.shape[0] # nc = image.shape[1] nr = len(image) nc = len(image[0]) for i in range(nr): for j in range(nc): if image[i,j]>throld: th[i,j] = 0 rgb = cv2.imread('test/rgb2.png',-1) depth = cv2.imread('test/去除地面_depth2.png',-1) thresh = 20 #二值化参数 max_thresh = 255 #二值化参数 #min_area = 500 #凸包的最小有效面积 min_area = 1400 dep = depth.copy() #讲depth复制给dep #筛选掉安全距离以外的点 mask_depth(depth,dep,1300) #cv2.imshow('depth_',depth) cv2.imshow('depth',dep) #显示去除安全距离以外的点的图像 cv2.waitKey(0) cv2.imwrite('one.png',dep) #转换数据类型 """ img_gray = cv.CreateMat(nr,nc,cv.CV_8UC1) print img_gray cv.CvtColor(dep,img_gray,cv.CV_BGR2GRAY) """ #dep.convertTO(dep,CV_8UC1, 1.0 / 16, 0) #进行开运算 kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(15,15)) opening = cv2.morphologyEx(dep,cv2.MORPH_OPEN,kernel) cv2.imshow('opoening',opening) print opening[100,100] cv2.waitKey(0) cv2.imwrite('opening.png',opening) #图像二值化 #img = cv2.imread('opening.png',-1) #gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) out = np.uint8(opening) ret, binary = cv2.threshold(out,127,255,cv2.THRESH_BINARY) #ret, binary = cv2.threshold(img,127,255,cv2.THRESH_BINARY) contours, hierarchy = cv2.findContours(binary,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) hull=[] def convexHull(contours): for i in range(len(contours)): area = cv2.contourArea(contours[i]) if area>=min_area: hull.append(cv2.convexHull(contours[i])) return hull convexHull(contours) #print hull #hull = cv2.convexHull(contours[0])re #cv2.drawContours(img,contours,-1,(0,0,255),3) cv2.drawContours(rgb,hull,-1,(0,0,255),3) cv2.imshow('img', rgb) cv2.waitKey(0) #for i in range(len(contours)): # cv2.convexHull(contour[i]) cv2.imwrite('test/6_10_h=0.05.png',rgb)
去除地面
#!usr/bin/python # -*- coding: utf-8 -*- import cv2 camera_factor = 1000.0 camera_fx = 517.3 camera_fy = 516.5 camera_cx = 318.6 camera_cy = 255.3 depth = cv2.imread('test/depth2.png',-1)#flag备注为-1,表示读取原始数据不做任何修改 dep = depth.copy() #讲depth复制给dep a=[] def point(depth_x,depth_y): d = depth[depth_x,depth_y] p_z= d/camera_factor p_x= (depth_y-camera_cx)*p_z/camera_fx p_y= (depth_x-camera_cy)*p_z/camera_fy point = [p_x,p_y,p_z] return point #print point(100,100) def get_point(image): global nr global nc # nr = image.shape[0] # nc = image.shape[1] nr = len(image) nc = len(image[0]) for m in range(nr): for n in range(nc): d = image[m,n] #访问像素点 if d == 0: #如果像素点不存在 continue else: a.append(point(m,n)) get_point(depth) #print a min_y = a[0][1] for i in range(len(a)): y = a[i][1] if y<min_y: min_y = y print min_y max_y = a[0][1] for i in range(len(a)): y = a[i][1] if y>max_y: max_y = y #设定阈值 h = 0.2 h_ground=-0.4 #去掉地面函数 def remove_ground(image,th): global nr global nc # nr = image.shape[0] # nc = image.shape[1] nr = len(image) nc = len(image[0]) for m in range(nr): for n in range(nc): d = image[m,n] #访问像素点 if d == 0: #如果像素点不存在 continue else: #print point(m,n) Y = point(m,n)[1] """ filename="Y/"+".png" frgb=open('Y.txt','a') frgb.write(t+" "+filename+'\n') """ #print Y if max_y-Y<h: th[m,n] = 0 remove_ground(depth,dep) cv2.imshow('depth',depth) cv2.waitKey(0) cv2.imshow('dep',dep) cv2.imwrite('test/去除地面_depth2',dep) cv2.waitKey(0) print dep