├── README.md ├── demo ├── depth.png └── rgb.png ├── get_imgs_from_camera ├── generate_rbgdepth_imgs.py └── record_camera.py ├── lib ├── csv2depth.py ├── generate_pointcloud.py ├── params_camera.py ├── ransac.py └── rgbd2pointcloud.py ├── main.py └── results ├── depth_not_plane.png ├── depth_of_plane.png ├── mask_of_plane.png └── rgb_of_plane.png /README.md: -------------------------------------------------------------------------------- 1 | # Plane_Detection_RGBD 2 | Plane detection using single RGBD image with RANSAC 3 | 4 | ## Hardware 5 | * realsense d415 6 | 7 | ## Dependencies 8 | * Python 3 9 | * numpy 10 | * matplotlib 11 | * pickle 12 | * cv2 13 | * PIL 14 | * ros 15 | * pyrealsense 16 | 17 | ## Quick start 18 | ``` 19 | # starting record videos 20 | cd get_imgs_from_camera 21 | python3 record_camera.py 22 | 23 | # extract images from recorded file 24 | mkdir depth 25 | mkdir rgb 26 | python3 generate_rbgdepth_imgs.py 27 | 28 | # segmenting the plane in the refered image 29 | cd .. 30 | python3 main.py -rgb /DIR/TO/RBG/IMAGE -depth /DIR/TO/DEPTH/IMAGE 31 | 32 | # segmented result will be saved in file "output.ply", which can be viewed by meshlab 33 | ``` 34 | ## Demo 35 | ``` 36 | python3 main.py -rgb demo/rgb.png -depth demo/depth.png 37 | ``` 38 | PS : The rgb image and the depth image in the demo file do not match strictly... 39 | -------------------------------------------------------------------------------- /demo/depth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dulucas/plane_detection_RGBD/1ffc56c0da8b978b8993c2990f51f7e1b68d4923/demo/depth.png -------------------------------------------------------------------------------- /demo/rgb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dulucas/plane_detection_RGBD/1ffc56c0da8b978b8993c2990f51f7e1b68d4923/demo/rgb.png -------------------------------------------------------------------------------- /get_imgs_from_camera/generate_rbgdepth_imgs.py: -------------------------------------------------------------------------------- 1 | import os 2 | os.system('rs-convert -i object_detection.bag -d -v depth/') 3 | os.system('rs-convert -i object_detection.bag -c -p rgb/') 4 | -------------------------------------------------------------------------------- /get_imgs_from_camera/record_camera.py: -------------------------------------------------------------------------------- 1 | import pyrealsense2 as rs 2 | import numpy as np 3 | import cv2 4 | 5 | # Configure depth and color streams 6 | pipeline = rs.pipeline() 7 | config = rs.config() 8 | config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) 9 | config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) 10 | config.enable_record_to_file('object_detection.bag') 11 | 12 | # Start streaming 13 | pipeline.start(config) 14 | 15 | e1 = cv2.getTickCount() 16 | 17 | try: 18 | while True: 19 | 20 | # Wait for a coherent pair of frames: depth and color 21 | frames = pipeline.wait_for_frames() 22 | depth_frame = frames.get_depth_frame() 23 | color_frame = frames.get_color_frame() 24 | if not depth_frame or not color_frame: 25 | continue 26 | 27 | # Convert images to numpy arrays 28 | depth_image = np.asanyarray(depth_frame.get_data()) 29 | color_image = np.asanyarray(color_frame.get_data()) 30 | 31 | # Apply colormap on depth image (image must be converted to 8-bit per pixel first) 32 | depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), 33 | cv2.COLORMAP_JET) 34 | 35 | # Stack both images horizontally 36 | images = np.hstack((color_image, depth_colormap)) 37 | 38 | # Show images 39 | cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) 40 | cv2.imshow('RealSense', images) 41 | cv2.waitKey(1) 42 | e2 = cv2.getTickCount() 43 | t = (e2 - e1) / cv2.getTickFrequency() 44 | if t > 15: # change it to record what length of video you are interested in 45 | print("Done!") 46 | break 47 | 48 | finally: 49 | 50 | # Stop streaming 51 | pipeline.stop() 52 | -------------------------------------------------------------------------------- /lib/csv2depth.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import numpy as np 3 | from lib.params_camera import config 4 | 5 | def csv2depth(name): 6 | result = [] 7 | with open(name, mode='r') as csv_file: 8 | csv_reader = csv.reader(csv_file) 9 | for row in csv_reader: 10 | result.append(row) 11 | result = np.array(result, dtype = np.float64) 12 | result[result > config.threhold_depth] = 0 13 | return result 14 | -------------------------------------------------------------------------------- /lib/generate_pointcloud.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2013, Juergen Sturm, TUM 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of TUM nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # the resulting .ply file can be viewed for example with meshlab 35 | # sudo apt-get install meshlab 36 | 37 | """ 38 | This script reads a registered pair of color and depth images and generates a 39 | colored 3D point cloud in the PLY format. 40 | """ 41 | 42 | import argparse 43 | import sys 44 | import os 45 | from PIL import Image 46 | from lib.params_camera import config 47 | 48 | def generate_pointcloud(rgb_file,depth_file,ply_file): 49 | """ 50 | Generate a colored point cloud in PLY format from a color and a depth image. 51 | 52 | Input: 53 | rgb_file -- filename of color image 54 | depth_file -- filename of depth image 55 | ply_file -- filename of ply file 56 | 57 | """ 58 | rgb = Image.open(rgb_file) 59 | depth = Image.open(depth_file) 60 | 61 | if rgb.size != depth.size: 62 | raise Exception("Color and depth image do not have the same resolution.") 63 | if rgb.mode != "RGB": 64 | raise Exception("Color image is not in RGB format") 65 | if depth.mode != "I": 66 | raise Exception("Depth image is not in intensity format") 67 | 68 | 69 | points = [] 70 | for v in range(rgb.size[1]): 71 | for u in range(rgb.size[0]): 72 | color = rgb.getpixel((u,v)) 73 | Z = depth.getpixel((u,v)) / config.scalingfactor 74 | if Z==0: continue 75 | X = (u - config.centerX) * Z / config.focal_length 76 | Y = (v - config.centerY) * Z / config.focal_length 77 | points.append("%f %f %f %d %d %d 0\n"%(X,Y,Z,color[0],color[1],color[2])) 78 | file = open(ply_file,"w") 79 | file.write('''ply 80 | format ascii 1.0 81 | element vertex %d 82 | property float x 83 | property float y 84 | property float z 85 | property uchar red 86 | property uchar green 87 | property uchar blue 88 | property uchar alpha 89 | end_header 90 | %s 91 | '''%(len(points),"".join(points))) 92 | file.close() 93 | 94 | if __name__ == '__main__': 95 | parser = argparse.ArgumentParser(description=''' 96 | This script reads a registered pair of color and depth images and generates a colored 3D point cloud in the 97 | PLY format. 98 | ''') 99 | parser.add_argument('rgb_file', help='input color image (format: png)') 100 | parser.add_argument('depth_file', help='input depth image (format: png)') 101 | parser.add_argument('ply_file', help='output PLY file (format: ply)') 102 | args = parser.parse_args() 103 | 104 | generate_pointcloud(args.rgb_file,args.depth_file,args.ply_file) 105 | 106 | -------------------------------------------------------------------------------- /lib/params_camera.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | from easydict import EasyDict as edict 4 | 5 | C = edict() 6 | config = C 7 | C.user = 'Robot master' 8 | 9 | # Params of camera 10 | C.focal_length = 500 11 | C.centerX = 240 12 | C.centerY = 320 13 | C.scalingfactor = 1000 14 | 15 | # File path of the images 16 | C.target_dir = 'results/' 17 | C.source_dir = 'XXX' 18 | 19 | # Params for Ransac 20 | C.num_iters_for_plane = 1 # number of times for the 3 points selection to define the plane 21 | C.num_iters_for_points = 50 # number of times for random selection of points on the plane 22 | C.error_tolerated = 5e-5 23 | C.num_groups = 300 # number of selected groups for every iteration 24 | C.threhold_percentage = 0.6 # threhold of percentage to admit a plane 25 | C.threhold_depth = 3 26 | C.threhold_mesh = 1000 27 | C.kmeans_clusters = 5 28 | -------------------------------------------------------------------------------- /lib/ransac.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import random 3 | from lib.params_camera import config 4 | 5 | def ransac(pointcloud): 6 | count = 0 7 | best_plane_detected = [] 8 | pointcloud = list(pointcloud) 9 | best_plane = None 10 | best_num_inliers = 0 11 | while count < config.num_iters_for_points: 12 | point_group = pointcloud # random.sample(pointcloud, config.num_groups) 13 | iters = 0 14 | while iters < config.num_iters_for_plane: 15 | num_inliers = 0 16 | random.shuffle(point_group) 17 | point_plane = point_group[:3] 18 | rest_point = point_group[3:] 19 | plane = estimate(point_plane) 20 | for point in rest_point: 21 | if is_inlier(point, plane, config.error_tolerated): 22 | num_inliers += 1 23 | if num_inliers > best_num_inliers: 24 | best_plane = plane 25 | best_num_inliers = num_inliers 26 | if num_inliers / config.num_groups > config.threhold_percentage: 27 | break 28 | iters += 1 29 | count += 1 30 | return best_plane, best_num_inliers / len(rest_point) 31 | 32 | def solvequation(point_plane): 33 | point0, point1, point2 = point_plane[0], point_plane[1], point_plane[2] 34 | matrix_A = np.array([point0, point1, point2]) 35 | matrix_A = matrix_A.reshape((3,3)) 36 | #vector_b = np.zeros(3) 37 | plane_coeff = np.linalg.svd(matrix_A)[-1][-1, :] 38 | return plane_coeff 39 | 40 | def estimate_inlier(point, plane): 41 | point = np.array(point).reshape((3,1)) 42 | plane = np.array(plane).reshape((1,3)) 43 | return np.dot(plane, point) < config.error_tolerated 44 | 45 | def augment(xyzs): 46 | axyz = np.ones((len(xyzs), 4)) 47 | axyz[:, :3] = xyzs 48 | return axyz 49 | 50 | def is_inlier(xyz, coeffs, threshold): 51 | return np.abs(coeffs.dot(augment([xyz]).T)) < threshold 52 | 53 | def estimate(xyzs): 54 | axyz = augment(xyzs[:3]) 55 | return np.linalg.svd(axyz)[-1][-1, :] 56 | -------------------------------------------------------------------------------- /lib/rgbd2pointcloud.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from lib.params_camera import config 3 | 4 | ### source code : https://svncvpr.in.tum.de/cvpr-ros-pkg/trunk/rgbd_benchmark/rgbd_benchmark_tools/src/rgbd_benchmark_tools/generate_pointcloud.py ### 5 | 6 | """ 7 | This script reads a registered pair of color and depth images and generates a 8 | colored 3D point cloud. 9 | """ 10 | 11 | def rgbd2pointcloud(rgb, depth, debug_mode = False): 12 | if rgb.shape[:2] != depth.shape: 13 | raise Exception("Color and depth image do not have the same resolution.") 14 | points = [] 15 | dict_depth_point = {} 16 | for v in range(rgb.shape[1]): 17 | for u in range(rgb.shape[0]): 18 | color = rgb[u,v] 19 | Z = depth[u,v] / config.scalingfactor 20 | if debug_mode: 21 | print('scaled depth : ', Z) 22 | if Z == 0: 23 | continue 24 | X = (u - config.centerX) * Z / config.focal_length 25 | Y = (v - config.centerY) * Z / config.focal_length 26 | points.append([X, Y, Z]) 27 | dict_depth_point[str([u, v, depth[u,v]])] = np.array([X, Y, Z, 1]).reshape((4,1)) 28 | return points, dict_depth_point 29 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import argparse 3 | import pickle as pkl 4 | from PIL import Image 5 | import matplotlib.pyplot as plt 6 | import random 7 | import cv2 8 | 9 | from lib.rgbd2pointcloud import rgbd2pointcloud 10 | from lib.ransac import ransac 11 | from lib.ransac import is_inlier 12 | from lib.csv2depth import csv2depth 13 | from lib.params_camera import config 14 | from lib.generate_pointcloud import generate_pointcloud 15 | 16 | def make_parser(): 17 | parser = argparse.ArgumentParser() 18 | parser.add_argument( 19 | '-rgb', '--rgbimage', default='demo/rgb.png') 20 | parser.add_argument( 21 | '-depth', '--depthimage', default='demo/depth.png') 22 | return parser 23 | 24 | def plot_plane(a, b, c, d): 25 | xx, yy = np.mgrid[:5, :5] 26 | return xx, yy, (-d - a * xx - b * yy) / c 27 | 28 | def rotate_view(vis): 29 | ctr = vis.get_view_control() 30 | ctr.rotate(10.0, 0.0) 31 | return False 32 | 33 | if __name__ == '__main__': 34 | parser = make_parser() 35 | args = parser.parse_args() 36 | rgb = cv2.imread(args.rgbimage) 37 | depth = plt.imread(args.depthimage) 38 | #depth = csv2depth(args.depthimage) 39 | depth = depth / 255 * 1000 40 | depth[depth>3] = 0 41 | #depth = plt.imread(args.depthimage) 42 | #depth = pkl.load(open('depth.pkl','rb')) 43 | rgb = rgb[:depth.shape[0], :depth.shape[1]] 44 | rgb = np.array(rgb, dtype=np.float64) 45 | mask = np.ones(depth.shape) 46 | mask[depth==0] = 0 47 | pointcloud, dict_depth_point = rgbd2pointcloud(rgb, depth) 48 | pointcloud_ori = pointcloud.copy() 49 | pointcloud = random.sample(list(pointcloud), 500) 50 | pointcloud = np.array(pointcloud).reshape((-1,3)) 51 | 52 | plane_detected, percentage_points_covered = ransac(pointcloud) 53 | for key in dict_depth_point.keys(): 54 | if np.dot(plane_detected, dict_depth_point[key]) < config.error_tolerated: 55 | coordinates = eval(str(key)) 56 | mask[coordinates[0], coordinates[1]] = 0 57 | depth_of_plane = depth * mask 58 | depth_not_plane = depth * (1 - mask) 59 | rgb_of_plane = rgb.copy() 60 | for i in range(3): 61 | rgb_of_plane[:, :, i] *= mask 62 | rgb_of_plane = np.array(rgb_of_plane, dtype=np.uint8) 63 | plt.imsave(config.target_dir + 'rgb_of_plane.png', rgb_of_plane) 64 | plt.imsave(config.target_dir + 'depth_of_plane.png', depth_of_plane) 65 | plt.imsave(config.target_dir + 'depth_not_plane.png', depth_not_plane) 66 | plt.imsave(config.target_dir + 'mask_of_plane.png', mask) 67 | im = Image.open(config.target_dir+"rgb_of_plane.png").convert('RGB') 68 | im.save(config.target_dir+'rgb_of_plane.png') 69 | im_ = Image.open(config.target_dir+"depth_of_plane.png").convert('I') 70 | im_.save(config.target_dir+'depth_of_plane.png') 71 | generate_pointcloud(config.target_dir+'rgb_of_plane.png', config.target_dir+'depth_of_plane.png', 'output.ply') 72 | -------------------------------------------------------------------------------- /results/depth_not_plane.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dulucas/plane_detection_RGBD/1ffc56c0da8b978b8993c2990f51f7e1b68d4923/results/depth_not_plane.png -------------------------------------------------------------------------------- /results/depth_of_plane.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dulucas/plane_detection_RGBD/1ffc56c0da8b978b8993c2990f51f7e1b68d4923/results/depth_of_plane.png -------------------------------------------------------------------------------- /results/mask_of_plane.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dulucas/plane_detection_RGBD/1ffc56c0da8b978b8993c2990f51f7e1b68d4923/results/mask_of_plane.png -------------------------------------------------------------------------------- /results/rgb_of_plane.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dulucas/plane_detection_RGBD/1ffc56c0da8b978b8993c2990f51f7e1b68d4923/results/rgb_of_plane.png --------------------------------------------------------------------------------