读取opencv彩色图和深度图并提取障碍物

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