Example #1
0
# Import PCL module
import pcl

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

################################################
# Voxel Grid filter

# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()

# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size
# Experiment and find the appropriate size!
LEAF_SIZE = 0.01

# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)

# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
filename = 'voxel_downsampled.pcd'
pcl.save(cloud_filtered, filename)

###################################################################
# PassThrough filter

# Create a PassThrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()
Example #2
0
 def setUp(self):
     self.p = pcl.load_XYZRGB("tests" + os.path.sep + "flydracyl.pcd")
Example #3
0
from __future__ import print_function

import numpy as np
import pcl

import pcl.pcl_visualization
# from pcl.pcl_registration import icp, gicp, icp_nl

cloud = pcl.load_XYZRGB(
    './examples/pcldata/tutorials/table_scene_mug_stereo_textured.pcd')
visual = pcl.pcl_visualization.CloudViewing()

# PointXYZ
# visual.ShowMonochromeCloud(cloud)

# visual.ShowGrayCloud(cloud, b'cloud')
visual.ShowColorCloud(cloud, b'cloud')
# visual.ShowColorACloud(cloud, b'cloud')

# while True:
#     visual.WasStopped()
# end

flag = True
while flag:
    flag != visual.WasStopped()
end
def random_color_gen():
    """ Generates a random color

        Args: None

        Returns:
            list: 3 elements, R, G, and B
    """
    r = randint(0, 255)
    g = randint(0, 255)
    b = randint(0, 255)
    return [r, g, b]


# Load Point Cloud file
cloud = pcl.load_XYZRGB('camera.pcd')
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(20)
x = 0.05
outlier_filter.set_std_dev_mul_thresh(x)
cloud = outlier_filter.filter()

### Voxel Grid filter
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.007
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud = vox.filter()

### PassThrough filter
passthrough = cloud.make_passthrough_filter()
filter_axis = 'z'
Example #5
0
 def setUp(self):
     self.p = pcl.load_XYZRGB("tests" + os.path.sep +
                              "table_scene_mug_stereo_textured_noplane.pcd")
     self.tmpdir = tempfile.mkdtemp(suffix='pcl-test')
Example #6
0
# -*- coding: utf-8 -*-
from __future__ import print_function

import numpy as np
import pcl

from pcl import pcl_visualization
# from pcl.pcl_registration import icp, gicp, icp_nl

#p = pcl.PointCloud()
#p.from_file("office.pcd")

cloud = pcl.load_XYZRGB('office.pcd')
visual = pcl.pcl_visualization.CloudViewing()

# PointXYZ
# visual.ShowMonochromeCloud(cloud)

# visual.ShowGrayCloud(cloud, b'cloud')
visual.ShowColorCloud(cloud, b'cloud')
# visual.ShowColorACloud(cloud, b'cloud')

# while True:
#     visual.WasStopped()
# end

flag = True
while flag:
    flag != visual.WasStopped()
end
Example #7
0
# -*- coding: utf-8 -*-

import pcl

# Cargar la nube de puntos
cloud = pcl.load_XYZRGB('../data/mesa.pcd')

# Crear un filtro VoxelGrid para la nube de puntos
fvox = cloud.make_voxel_grid_filter()
# Tamaño de voxel
VOXEL_SIZE = 1
# Asignar el tamaño del voxel al filtro
fvox.set_leaf_size(VOXEL_SIZE, VOXEL_SIZE, VOXEL_SIZE)

# Ejecutar el filtro
cloud_filtered = fvox.filter()

# Grabar el resultado en disco
filename = 'mesa_downsampled.pcd'
pcl.save(cloud_filtered, filename)

Example #8
0
"""lets create a working code"""

from sklearn.cluster import KMeans
import pcl
from pcl import pcl_visualization
cloud3 = pcl.PointCloud_PointXYZRGB()
point_cloud = pcl.PointCloud()
import numpy as np
import matplotlib.pyplot as plt

#cloud = pcl.load_XYZRGB('/home/ribin/Downloads/object_02/test58.pcd')
#path = "/home/ribin/Desktop/pcdfiles"

cloud = pcl.load_XYZRGB(
    '/media/ribin/DATA/addverb/binpicking/pcd_files/object_pcd/biscut/2.pcd')
print(cloud)
#   print(cloud.shape)
cloud2 = np.zeros((307201, 4), dtype=np.float32)
#cloud2 = np.array(cloud2)


def do_passthrough_filter(point_cloud,
                          name_axis='z',
                          min_axis=0.01,
                          max_axis=1):
    pass_filter = point_cloud.make_passthrough_filter()
    pass_filter.set_filter_field_name(name_axis)
    pass_filter.set_filter_limits(min_axis, max_axis)
    return pass_filter.filter()

    segmenter.set_model_type(pcl.SACMODEL_PLANE)
    segmenter.set_method_type(pcl.SAC_RANSAC)
    segmenter.set_distance_threshold(max_distance)

    #obtain inlier indices and model coefficients
    inlier_indices, coefficients = segmenter.segment()

    inliers = point_cloud.extract(inlier_indices, negative=False)
    outliers = point_cloud.extract(inlier_indices, negative=True)

    return inliers, outliers


##################################################################################
# This pipeline separates the objects in the table from the given scene

# Load the point cloud in memory
cloud = pcl.load_XYZRGB('point_clouds/tabletop.pcd')

# Get only information in our region of interest, as we don't care about the other parts
filtered_cloud = do_passthrough_filter(point_cloud=cloud,
                                       name_axis='z',
                                       min_axis=0.6,
                                       max_axis=1.1)

# Separate the table from everything else
table_cloud, objects_cloud = do_ransac_plane_segmentation(filtered_cloud,
                                                          max_distance=0.01)
pcl.save(objects_cloud, 'objects.pcd')
Example #10
0
def DBScan():

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

	# Voxel Grid Downsampling filter
	################################
	# Create a VoxelGrid filter object for our input point cloud
	vox = cloud.make_voxel_grid_filter()

	# Choose a voxel (also known as leaf) size
	# Note: this (1) means 1mx1mx1m is a poor choice of leaf size   
	# Experiment and find the appropriate size!
	#LEAF_SIZE = 0.01   
	LEAF_SIZE =45

	# Set the voxel (or leaf) size  
	vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)

	# Call the filter function to obtain the resultant downsampled point cloud
	cloud_filtered = vox.filter()
	filename = './pcd_out/voxel_downsampled.pcd'
	pcl.save(cloud_filtered, filename)

	# PassThrough filter
	################################
	# Create a PassThrough filter object.
	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
	axis_max = 100
	passthrough.set_filter_limits(axis_min, axis_max)

	# Finally use the filter function to obtain the resultant point cloud. 
	cloud_filtered = passthrough.filter()
	filename = './pcd_out/pass_through_filtered.pcd'
	pcl.save(cloud_filtered, filename)
	# RANSAC plane segmentation
	################################
	# Create the segmentation object
	seg = cloud_filtered.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 outliers
	# Save pcd for tabletop objects
	################################
	extracted_outliers = cloud_filtered.extract(inliers, negative=True)
	e=np.asarray(extracted_outliers)
	#print e[:,:-1]
	filename = './pcd_out/extracted_outliers.pcd'
	pcl.save(extracted_outliers, filename)

	# Generate some clusters!
	data = e[:,:-1]
	return(data)
Example #11
0
# -*- coding: utf-8 -*-
from __future__ import print_function

import numpy as np
import pcl

import pcl.pcl_visualization
# from pcl.pcl_registration import icp, gicp, icp_nl

cloud = pcl.load_XYZRGB(
    '/home/dllab/kitti_object/data_object_velodyne/pcl/1.pcd')
visual = pcl.pcl_visualization.CloudViewing()

# PointXYZ
# visual.ShowMonochromeCloud(cloud)

# visual.ShowGrayCloud(cloud, b'cloud')
visual.ShowColorCloud(cloud, b'cloud')
# visual.ShowColorACloud(cloud, b'cloud')

# while True:
#     visual.WasStopped()
# end

flag = True
while flag:
    flag != visual.WasStopped()
end
            #     print(type(make_label))
            #     print("label", label)
            #     print("label pos",label_pos)
            #     print("index",index)
            #     print(make_label(label,label_pos, index))
            #     object_markers_pub.publish(make_label(label,label_pos, index))
            #
            #     # Add the detected object to the list of detected objects.
            #     do = DetectedObject()
            #     do.label = label
            #     do.cloud = ros_cluster
            #     detected_objects.append(do)

            # Publish the list of detected objects

            # Suggested location for where to invoke your pr2_mover() function within pcl_callback()
            # Could add some logic to determine whether or not your object detections are robust
            # before calling pr2_mover()
            # try:
            #     pr2_mover(detected_objects_list)
            # except rospy.ROSInterruptException:
            #     pass


if __name__ == '__main__':
    cloud = pcl.load_XYZRGB('sample_pcd_files/left_cloud.pcd')

    get_color_list.color_list = []

    pcl_callback(cloud)
#!/usr/bin/env python

# Import modules
from pcl_helper import *

import pcl

#rospy.init_node('clustering', anonymous=True)
get_color_list.color_list =[]

# Load Point Cloud file
cloud = pcl.load_XYZRGB('tabletop.pcd')  # <type 'pcl._pcl.PointCloud_PointXYZRGB'>
#print cloud.size  # 202627,  each row is a 4-element tuple

# TODO: Voxel Grid Downsampling
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.01   
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()

# TODO: PassThrough Filter
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.5
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()

# TODO: RANSAC Plane Segmentation
seg = cloud_filtered.make_segmenter()
Example #14
0
#Import PCL 

import pcl 

#load Point Cloud File 

cloud = pcl.load_XYZRGB('filename.pcd')

#Voxel Grid filter

#a VoxelGrid filter object for the input cloud

vox = cloud.make_voxel_grid_filter()

########################################
#Tune Voxel Parameters

#voxel size
n = 0.01
leaf_size = n 

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

#set the voxel with the assigned leaf_size 

vox.set_leaf_size(leaf_size, leaf_size, leaf_size)

#call filter function to obtain the resultant downsampled point cloud

cloud_filtered = vox.filter()
Example #15
0
import pcl
import numpy as np
from sensor_stick.pcl_helper import *
# p_base
p_robot = np.array([[0.27265, -0.65591, 0.16760, 1],
                    [0.35291, -0.65217, 0.13476, 1],
                    [0.33833, -0.71059, 0.14230, 1],
                    [0.27451, -0.71246, 0.13478, 1]])
p_robot = p_robot.transpose()

p_camera = []

# process point cloud
for i in range(4):
    # load obtained point cloud
    cloud = pcl.load_XYZRGB('waypoint%s.ply' % (i + 1))
    # TODO: PassThrough Filter
    # roughly slicing for the point cloud
    passthrough = cloud.make_passthrough_filter()
    # filtering in z axis from 380 to 435
    filter_axis = 'z'
    passthrough.set_filter_field_name(filter_axis)
    axis_min = 380
    axis_max = 435
    passthrough.set_filter_limits(axis_min, axis_max)
    cloud_filtered = passthrough.filter()

    # TODO: Statistical outlier filter
    outlier_filter = cloud_filtered.make_statistical_outlier_filter()
    outlier_filter.set_mean_k(50)
    outlier_filter.set_std_dev_mul_thresh(1.0)
#this is a working program for cylinder segmentation

import pcl
from pcl import pcl_visualization

cloud = pcl.load("/home/ribin/Downloads/object_02/test58.pcd")
cloud1 = pcl.load_XYZRGB('/home/ribin/Downloads/object_02/test58.pcd')

print('PointCloud has: ' + str(cloud.size) + ' data points.')
"""
passthrough = cloud.make_passthrough_filter()
passthrough.set_filter_field_name ('z')
passthrough.set_filter_limits (0, 1.5)
cloud_filtered = passthrough.filter()
print('PointCloud has: ' + str(cloud_filtered.size) + ' data points.')
"""
# Estimate point normals
# ne.setSearchMethod (tree);
# ne.setInputCloud (cloud_filtered);
# ne.setKSearch (50);
# ne.compute (*cloud_normals);
#ne = cloud_filtered.make_NormalEstimation()
#tree = cloud_filtered.make_kdtree()
ne = cloud.make_NormalEstimation()
tree = cloud.make_kdtree()

ne.set_SearchMethod(tree)
ne.set_KSearch(50)
# cloud_normals = ne.compute ()

seg = cloud.make_segmenter_normals(ksearch=50)
Example #17
0
# Import PCL module
import pcl
# Load Point Cloud file
cloud = pcl.load_XYZRGB("tabletop.pcd")

# STEP 1 : Voxel Grid filter

# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()
# Choose a voxel (also known as leaf) size
# Note: this (0.01) is a decent choice of leaf size
LEAF_SIZE = 0.01
# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
# Save the Downsampled Point Cloud
filename = 'voxel_downsampled.pcd'
pcl.save(cloud_filtered, filename)

# STEP 2 : PassThrough filter

# Create a PassThrough filter object.
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.
# Import PCL module
import pcl

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

# Voxel Grid filter
# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()

# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size
# Experiment and find the appropriate size!
LEAF_SIZE = 0.003  #reasonable voxel(leaf) size

# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)

# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
#filename = 'voxel_downsampled.pcd'
#pcl.save(cloud_filtered, filename)

# PassThrough filter
# Create a PassThrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()

# Assign axis and range to the passthrough filter object.
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -0.5
Example #19
0
import pcl

cloud = pcl.load_XYZRGB('Aubo_tool0_exposure10.ply')

# TODO: PassThrough Filter
passthrough = cloud.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.4
axis_max = 0.435
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()

# TODO: Statistical outlier filter
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(50)
outlier_filter.set_std_dev_mul_thresh(1.0)
cloud_filtered = outlier_filter.filter()

# TODO: RANSAC Plane Segmentation
seg = cloud_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
max_distance = 0.01
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()

# TODO: Extract inliers and outliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
Example #20
0
# Import PCL module
import pcl

# Import modules
from pcl_helper import *

# Load Point Cloud file
cloud_filtered = pcl.load_XYZRGB('voxel_downsampled.pcd')
"""
###################
# Voxel Grid filter
###################

#Create a VoxelGrid filter object four input point cloud
vox=cloud.make_voxel_grid_filter()

# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size   
# Experiment and find the appropriate size!
LEAF_SIZE = 0.01

#Set the voxel (of leaf) size
vox.set_leaf_size(LEAF_SIZE,LEAF_SIZE,LEAF_SIZE)

#Call the filiter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
filename = 'voxel_downsampled.pcd'
pcl.save(cloud_filtered, filename)
"""
#####################
# PassThrough filter
Example #21
0
import numpy.core.multiarray
import pcl
from pcl import pcl_visualization
cloud3 = pcl.PointCloud_PointXYZRGB()
point_cloud = pcl.PointCloud()
import numpy as np
#import  pandas as pd

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

#import matplotlib.pyplot as plt
#from mpl_toolkits.mplot3d import Axes3D

#cloud = pcl.load_XYZRGB('/home/ribin/Downloads/object_02/test58.pcd')
cloud = pcl.load_XYZRGB('/home/ribin/Downloads/mug.pcd')
#   print(cloud.shape)
cloud2 = np.zeros((307201, 4), dtype=np.float32)
#cloud2 = np.array(cloud2)


def do_passthrough_filter(point_cloud,
                          name_axis='z',
                          min_axis=0.01,
                          max_axis=1):
    pass_filter = point_cloud.make_passthrough_filter()
    pass_filter.set_filter_field_name(name_axis)
    pass_filter.set_filter_limits(min_axis, max_axis)
    return pass_filter.filter()

Example #22
0
#!/usr/bin/python
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt
import cv2
from sklearn.cluster import DBSCAN
from extra_functions import cluster_gen
import pcl
import numpy as np
import matplotlib.cm as cm
import data_image_2
import itertools
# get pcd file
data_image_2.plot()
# Load Point Cloud file
cloud = pcl.load_XYZRGB('./test_rgb.pcd')

# Voxel Grid Downsampling filter
################################
# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()

# Choose a voxel (also known as leaf) size
# Note: this (1) means 1mx1mx1m is a poor choice of leaf size   
# Experiment and find the appropriate size!
#LEAF_SIZE = 0.01   
LEAF_SIZE =45

# Set the voxel (or leaf) size  
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
Example #23
0
# -*- coding: utf-8 -*-
"""
Created on Tue Dec 11 15:20:49 2018

@author: bimta
"""

import pcl

cloud = pcl.load_XYZRGB('./Data/poles.pcd')
print(cloud.size)


def subtractGround(pointcloud):
    seg = pointcloud.make_segmenter_normals(ksearch=50)
    seg.set_optimize_coefficients(True)
    seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
    seg.set_normal_distance_weight(0.1)
    seg.set_method_type(pcl.SAC_RANSAC)
    seg.set_max_iterations(10000)
    seg.set_distance_threshold(0.3)
    indices, model = seg.segment()
    #print(model)
    #cloud_plane = pointcloud.extract(indices, negative=False)
    cloud_without_plane = pointcloud.extract(indices, negative=True)
    return cloud_without_plane


#TODO: filter for cylinder axes with certain angle
def findCylinder(pointcloud):
    seg = pointcloud.make_segmenter_normals(ksearch=50)
    plt.xlabel('Predicted label')


if __name__ == "__main__":
    rospy.init_node('train_from_file_node')
    pcl_objects_pub = rospy.Publisher("/pr2/pcl_objects",
                                      PointCloud2,
                                      queue_size=1)

    # Features
    labeled_features = []

    for model_name in MODELS_3:
        print "Features: %s..." % model_name
        for c_url in glob.glob(MODELS_URL + model_name + "/*.pcd"):
            cloud = pcl.load_XYZRGB(c_url)
            cloud_r = pcl_to_ros(cloud)

            pcl_objects_pub.publish(cloud_r)

            # chists = compute_color_histograms(cloud_r, using_hsv=True)
            # nhists = compute_normal_histograms(get_normals(cloud_r))
            # bounds = compute_size(cloud_r)
            # feature = np.concatenate((chists, nhists, bounds))
            feature = compute_all_features(cloud_r, get_normals(cloud_r))
            labeled_features.append([feature, model_name])

    pickle.dump(labeled_features, open('training_set.sav', 'wb'))

    # Train SVM
    # Format the features and labels for use with scikit learn
Example #25
0
 def testSave(self):
     for ext in ["pcd", "ply"]:
         d = os.path.join(self.tmpdir, "foo." + ext)
         pcl.save(self.p, d)
         p = pcl.load_XYZRGB(d)
         self.assertEqual(self.p.size, p.size)
Example #26
0
#import  pandas as pd

import matplotlib.pyplot as plt
import os
from mpl_toolkits.mplot3d import Axes3D


#import matplotlib.pyplot as plt
#from mpl_toolkits.mplot3d import Axes3D


#cloud = pcl.load_XYZRGB('/home/ribin/Downloads/object_02/test58.pcd')
#path = "/home/ribin/Desktop/pcdfiles"


cloud = pcl.load_XYZRGB('/media/ribin/DATA/bagfiles/pcdfiles/Zed_new_HQ/135.pcd')
print(cloud)
#   print(cloud.shape)
cloud2 = np.zeros((307201,4),dtype=np.float32)
#cloud2 = np.array(cloud2)


def do_passthrough_filter(point_cloud, name_axis = 'y', min_axis = 0.01, max_axis = 1):
  pass_filter = point_cloud.make_passthrough_filter()
  pass_filter.set_filter_field_name(name_axis);
  pass_filter.set_filter_limits(min_axis, max_axis)
  return pass_filter.filter()


def do_voxel_grid_filter(point_cloud, LEAF_SIZE):
    voxel_filter = point_cloud.make_voxel_grid_filter()
Example #27
0
 def setUp(self):
     self.p = pcl.load_XYZRGB(
         "tests/table_scene_mug_stereo_textured_noplane.pcd")
#!/usr/bin/env python

import pcl
import numpy as np

cloud = pcl.load_XYZRGB("pass_through_filtered.pcd")
#pcl.getMinMax3D(cloud, min, max)
#print(cloud[0])
cloud_array = np.array(cloud)
#print(type(cloud_array))
print('min: ', np.amin(cloud_array, axis=0))
print('max: ', np.amax(cloud_array, axis=0))
Example #29
0
def DBScan():

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

    # Voxel Grid Downsampling filter
    ################################
    # Create a VoxelGrid filter object for our input point cloud
    vox = cloud.make_voxel_grid_filter()

    # Choose a voxel (also known as leaf) size
    # Note: this (1) means 1mx1mx1m is a poor choice of leaf size
    # Experiment and find the appropriate size!
    #LEAF_SIZE = 0.01
    LEAF_SIZE = 45

    # Set the voxel (or leaf) size
    vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)

    # Call the filter function to obtain the resultant downsampled point cloud
    cloud_filtered = vox.filter()
    filename = './pcd_out/voxel_downsampled.pcd'
    pcl.save(cloud_filtered, filename)

    # PassThrough filter
    ################################
    # Create a PassThrough filter object.
    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
    axis_max = 100
    passthrough.set_filter_limits(axis_min, axis_max)

    # Finally use the filter function to obtain the resultant point cloud.
    cloud_filtered = passthrough.filter()
    filename = './pcd_out/pass_through_filtered.pcd'
    pcl.save(cloud_filtered, filename)
    # RANSAC plane segmentation
    ################################
    # Create the segmentation object
    seg = cloud_filtered.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 outliers
    # Save pcd for tabletop objects
    ################################
    extracted_outliers = cloud_filtered.extract(inliers, negative=True)
    e = np.asarray(extracted_outliers)
    #print e[:,:-1]
    filename = './pcd_out/extracted_outliers.pcd'
    pcl.save(extracted_outliers, filename)

    # Generate some clusters!
    data = e[:, :-1]
    #print data
    #file=open("data.txt","w")
    #file.write(data)
    #print type(data)
    # Define max_distance (eps parameter in DBSCAN())
    max_distance = 175
    #max_distance = 1000
    db = DBSCAN(eps=max_distance, min_samples=10).fit(data)
    # Extract a mask of core cluster members
    core_samples_mask = np.zeros_like(db.labels_, dtype=bool)
    core_samples_mask[db.core_sample_indices_] = True
    # Extract labels (-1 is used for outliers)
    labels = db.labels_
    n_clusters = len(set(labels)) - (1 if -1 in labels else 0)
    unique_labels = set(labels)
    #print unique_labels
    # Plot up the results!
    # The following is just a fancy way of plotting core, edge and outliers
    colors = cm.rainbow(np.linspace(0, 1, len(unique_labels)))
    array_x = []
    array_y = []
    array_z = []

    for k, col in zip(unique_labels, colors):
        #ax.clear()
        if k == -1:
            # Black used for noise.
            #col = [0, 0, 0, 1]
            col = (0, 0, 0, 1)

        class_member_mask = (labels == k)
        #print len(class_member_mask)
        xy2 = data[class_member_mask & core_samples_mask]
        array_x.extend(xy2[:, 0])
        array_y.extend(xy2[:, 1])
        array_z.extend(xy2[:, 2])
    stack_array = np.vstack((array_x, array_y, array_z))
    #print xy2
    return stack_array
Example #30
0
def random_color_gen():
    """ Generates a random color

        Args: None

        Returns:
            list: 3 elements, R, G, and B
    """
    r = randint(0, 255)
    g = randint(0, 255)
    b = randint(0, 255)
    return [r, g, b]


# Load Point Cloud file
cloud = pcl.load_XYZRGB('camera_right.pcd')
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(20)
x = 0.05
outlier_filter.set_std_dev_mul_thresh(x)
cloud = outlier_filter.filter()

### Voxel Grid filter
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.01
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud = vox.filter()

### 1st PassThrough filter in z-axis
passthrough = cloud.make_passthrough_filter()
filter_axis = 'z'
Example #31
0
# Import PCL module
import pcl

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

# Voxel Grid filter
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)

# PassThrough filter

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)
Example #32
0
# -*- coding: utf-8 -*-
# http://virtuemarket-lab.blogspot.jp/2015/03/harris.html

from __future__ import print_function

import numpy as np
import pcl

import pcl.pcl_visualization

# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
# pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud);
cloud = pcl.load_XYZRGB('G:\\tmp\\PCL\\extendlibrary\\python-pcl\\examples\\pcldata\\tutorials\\table_scene_mug_stereo_textured.pcd')

# pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI> detector;
# detector.setNonMaxSupression (true);
# detector.setRadius (0.01);
# //detector.setRadiusSearch (100);
# detector.setInputCloud(cloud);
detector = cloud.make_HarrisKeypoint3D()

# pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints(new pcl::PointCloud<pcl::PointXYZI>());
# detector.compute(*keypoints);
keypoints = detector.compute()

# std::cout << "keypoints detected: " << keypoints->size() << std::endl;

# pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>());
# pcl::PointXYZ tmp;
# double max = 0,min=0;
#