<reponame>AhmdNassar/3D-perception-exercises
# Import PCL module
import pcl

# Load Point Cloud file
cloud = pcl.load_XYZRGB('tabletop.pcd')


# Voxel Grid filter

######## comment as I did it #############
#vox = cloud.make_voxel_grid_filter()

#LEAF_SIZE = 0.01

#vox.set_leaf_size(LEAF_SIZE,LEAF_SIZE,LEAF_SIZE)

#cloud_filtered = vox.filter()

#filename = 'voxel_downsampled.pcd'

#pcl.save(cloud_filtered,filename)

###################################


###### comment as I did It ##################
# PassThrough filter
# Create a PassThrough filter object.

#cloud_filtered = pcl.load_XYZRGB('voxel_downsampled.pcd')

#passthrough = cloud_filtered.make_passthrough_filter()

# Assign axis and range to the passthrough filter object.
#filter_axis = 'z'
#passthrough.set_filter_field_name(filter_axis)
#axis_min = 0.6
#axis_max = 1.1
#passthrough.set_filter_limits(axis_min, axis_max)

# Finally use the filter function to obtain the resultant point cloud. 
#cloud_filtered = passthrough.filter()
#filename = 'pass_through_filtered.pcd'
#pcl.save(cloud_filtered, filename)

##################################

# RANSAC plane segmentation
cloud = pcl.load_XYZRGB('pass_through_filtered.pcd')
# Create the segmentation object
seg = cloud.make_segmenter()

# Set the model you wish to fit 
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)

# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance 
# for segmenting the table
max_distance = 0.01
seg.set_distance_threshold(max_distance)

# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()


# Extract inliers
extracted_inliers = cloud.extract(inliers, negative=False)
filename = 'extracted_inliers.pcd'
pcl.save(extracted_inliers, filename)

# Extract outliers
extracted_outliers = cloud.extract(inliers, negative=True)
filename = 'extracted_outliers.pcd'
pcl.save(extracted_outliers, filename)

# Save pcd for tabletop objects


