位置：/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
