def push_map(self): # Pushes map pointcloud to database and writes to local file
# Push map to server
# TODO: Push map to database computer
# Store map as local .pcd file
pcl.save(self.mapa, "map.pcd")
def main():
p = argparse.ArgumentParser(formatter_class=argparse.RawDescriptionHelpFormatter, description="")
p.add_argument('--v', dest = 'video_path', action = 'store', default = '', help = 'path file *.oni')
args = p.parse_args()
if not os.path.isdir(OUTDIR):
os.mkdir(OUTDIR)
i = 1
while os.path.isfile(DEPTHDIR + "/" + str(i) + EXT1):
depth_frame = cv2.imread(DEPTHDIR + "/"+str(i)+EXT1,2)
depth_frame = cv2.split(depth_frame)
depth_frame = depth_frame[0]
points = cvtDepthTo3DPoints(depth_frame, 1)
pointcloud = pcl.PointCloud()
pointcloud.from_array(points)
os.chdir(OUTDIR)
pcl.save(pointcloud, str(i) + EXT2)
pointcloud.to_file(str(i) + EXT3)
print "Converting " + str(i) + ".png" + " into " + str(i) + ".pcd and into " + str(i) + ".ply"
os.chdir("..")
i+=1
ch = 0xFF & cv2.waitKey(1)
if ch == 27:
break
cv2.destroyAllWindows()
def save(cloud, path, format=None, binary=False, las_header=None):
"""Save a pointcloud to file.
Supports LAS and CSV files, and lets PCD and PLY
files be saved by python-pcl.
Arguments:
cloud : pcl.PointCloud or pcl.PointCloudXYZRGB
Pointcloud to save.
path : string
Filename.
format : string
File format: "PLY", "PCD", "LAS", "CSV",
or None to detect the format from the file extension.
binary : boolean
Whether PLY and PCD files are saved in binary format.
las_header: liblas.header.Header
LAS header to use. When none, a default header is created by
make_las_header(). Default: None
"""
if format == 'las' or format is None and path.endswith('.las'):
_save_las(path, cloud, header=las_header)
elif format == 'csv' or format is None and path.endswith('.csv'):
_save_csv(path, cloud)
else:
_check_writable(path)
if is_registered(cloud) and cloud.offset != np.zeros(3):
cloud_array = np.asarray(cloud)
cloud_array += cloud.offset
pcl.save(cloud, path, format=format, binary=binary)
def testSave(self):
# for ext in ["pcd", "ply"]:
# Mac ply read/write NG
for ext in ["pcd"]:
d = os.path.join(self.tmpdir, "foo." + ext)
pcl.save(self.p, d)
p = pcl.load(d)
self.assertEqual(self.p.size, p.size)
def pull_map(): # Downloads map from database and writes it to local file
# Get map from database
# TODO: Get map from database computer
# Try to load map
try:
self.mapa = pcl.load("map.pcd")
except IOError:
# "empty" point cloud
self.mapa = pcl.PointCloud()
# Save to local file
pcl.save(self.mapa, "map.pcd")
cloud = vox.filter() ### 1st PassThrough filter in z-axis passthrough = cloud.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.4 axis_max = 5.0 passthrough.set_filter_limits(axis_min, axis_max) cloud = passthrough.filter() ### 2nd PassThrough filter in x-axis passthrough2 = cloud.make_passthrough_filter() filter_axis = 'x' passthrough2.set_filter_field_name(filter_axis) axis_min = -1.0 axis_max = 0.46 passthrough2.set_filter_limits(axis_min, axis_max) cloud = passthrough2.filter() ### 3rd PassThrough filter in y-axis passthrough3 = cloud.make_passthrough_filter() filter_axis = 'y' passthrough3.set_filter_field_name(filter_axis) axis_min = -5.0 axis_max = -0.43 passthrough3.set_filter_limits(axis_min, axis_max) cloud = passthrough3.filter() pcl.save(cloud, 'right_passthrough.pcd')
import pcl
##################################################################################
# This pipeline reduces the statistical noise of the scene
point_cloud = pcl.load("test.pcd")
noise_filter = point_cloud.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
noise_filter.set_mean_k(50)
# Any point with a mean distance larger than global (mean distance+x*std_dev)
# will be considered outlier
noise_filter.set_std_dev_mul_thresh(1.0)
pcl.save(noise_filter.filter(), "test.pcd")
noise_filter.set_negative(True)
pcl.save(noise_filter.filter(), "test.pcd")
# http://nisot0710.blogspot.jp/2014/09/pclvoxelgridpclpointcloud2.html
# PCLPointCloud2 is 1.7.2
import pcl
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
#
# pcl::PCDReader reader;
# reader.read("pcdfilename", *cloud);
cloud = pcl.load('./examples/pcldata/tutorials/table_scene_lms400.pcd')
# std::cerr<<"PointCloud befor filtering: " << cloud->width * cloud->height << "data points ( " << pcl::getFieldsList (*cloud) << ").";
# print('PointCloud befor filtering: ' + str(cloud.width * cloud.height) + 'data points ( ' + pcl.getFieldsList (cloud) + ').')
# pcl::VoxelGrid<pcl::PointXYZ> sor;
# sor.setInputCloud(cloud);
# sor.setLeafSize(0.1f, 0.1f, 0.1f);
# sor.filter(*cloud_filtered);
sor = cloud.make_voxel_grid_filter()
sor.set_leaf_size(0.1, 0.1, 0.1)
cloud_filtered = sor.filter()
# std::cerr<<"PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << "data points (" << pcl::getFieldsList(*cloud_filtered) <<").";
# print('PointCloud after filtering: ' + str(cloud_filtered.width * cloud_filtered.height) + 'data points ( ' + pcl.getFieldsList (cloud) + ').')
# pcl::PCDWriter writer;
# writer.write("savefilename", *cloud_filtered, false);
pcl.save(cloud_filtered, 'table_scene_lms400_voxelfilter.pcd')
pointcloud[i * 180 + j][0] = i
pointcloud[i * 180 + j][1] = j
pointcloud[i * 180 + j][2] = -dist * z_unit
if dist != 0 and not math.isnan(dist):
if dist < 0:
cv2.circle(color_image, (x, y), 1, (0, 0, 255), 1)
else:
cv2.circle(color_image, (x, y), 1, (255, 0, 0), 1)
point_array['x'].append(x)
point_array['y'].append(y)
pc = pcl.PointCloud(pointcloud)
pcl.save(pc, 'pc2pcd.pcd')
image = np.hstack((color_image, depth_colormap))
# cv2.namedWindow('Panorama View', cv2.WINDOW_NORMAL)
# cv2.imshow('Panorama View', image)
# cv2.waitKey(0)
cv2.imwrite('Image.jpg', image)
# DBSCAN
df = pd.DataFrame(point_array)
dbscan = DBSCAN(eps=EPS, min_samples=MIN_SAMPLES, metric='euclidean')
dbscan_labels = dbscan.fit_predict(df)
df['dbscan_cluster'] = dbscan_labels
visualize_cluster_plot(dbscan, df, 'dbscan_cluster', iscenter=False)
# Volume estimating
def _save_cloud(self, cloud, index):
file = self.temp_folder + "point_" + str(index) + ".ply"
print("save point cloud to ", file)
pcl.save(cloud, file, "ply")
# -*- 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)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
print "number of clusters:", len(cluster_indices)
#print cluster_indices
#Assign a color corresponding to each segmented object in scene
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
print j, len(indices)
for i, indice in enumerate(indices):
color_cluster_point_list.append([white_cloud[indice][0],
white_cloud[indice][1],
white_cloud[indice][2],
rgb_to_float(cluster_color[j])])
#Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
pcl.save(cluster_cloud,'b.pcd')
#pcl.save(extracted_outliers,'b.pcd')
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = extracted_outliers.extract(pts_list) # <type 'pcl._pcl.PointCloud_PointXYZRGB'>
#print type(pcl_cluster)
# -*- coding: utf-8 -*-
# port of
# http://pointclouds.org/documentation/tutorials/statistical_outlier.php
# you need to download
# http://svn.pointclouds.org/data/tutorials/table_scene_lms400.pcd
import pcl
p = pcl.load("./examples/pcldata/tutorials/table_scene_lms400.pcd")
fil = p.make_statistical_outlier_filter()
fil.set_mean_k(50)
fil.set_std_dev_mul_thresh(1.0)
pcl.save(fil.filter(), "./examples/pcldata/tutorials/table_scene_lms400_inliers.pcd")
fil.set_negative(True)
pcl.save(fil.filter(), "./examples/pcldata/tutorials/table_scene_lms400_outliers.pcd")
# sor.setLeafSize (0.01f, 0.01f, 0.01f);
# sor.filter (*cloud_filtered_blob);
sor = cloud_blob.make_voxel_grid_filter()
sor.set_leaf_size(0.01, 0.01, 0.01)
cloud_filtered_blob = sor.filter()
# Convert to the templated PointCloud
# pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
# std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
cloud_filtered = pcl.PCLPointCloud2(cloud_filtered_blob.to_array())
print('PointCloud after filtering: ' + str(cloud_filtered.width * cloud_filtered.height) + ' data points.')
# Write the downsampled version to disk
# pcl::PCDWriter writer;
# writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
pcl.save("table_scene_lms400_downsampled.pcd", cloud_filtered)
# pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
# pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
# // Create the segmentation object
# pcl::SACSegmentation<pcl::PointXYZ> seg;
# // Optional
# seg.setOptimizeCoefficients (true);
# // Mandatory
# seg.setModelType (pcl::SACMODEL_PLANE);
# seg.setMethodType (pcl::SAC_RANSAC);
# seg.setMaxIterations (1000);
# seg.setDistanceThreshold (0.01);
# // Create the filtering object
# pcl::ExtractIndices<pcl::PointXYZ> extract;
# lines = file.read()
# graph = jsonpickle.decode(lines)
counter_index = opts.start_frame_no # 1#21#30#x1
pcl_total_cloud = pcl.PointCloud().to_array()
while True:
file_string = "scan" + format(counter_index, "05d") + ".pcd"
file_string = os.path.join(opts.dir_prefix, file_string)
if not os.path.isfile(file_string):
break
pcl_cloud = pcl.load(file_string)
# print pcl_cloud
pcl_total_cloud = np.vstack((pcl_total_cloud, pcl_cloud.to_array()))
# print pcl.PointCloud().from_array(pcl_total_cloud)
# print pcl_total_cloud
print counter_index
counter_index = counter_index + 1
# print pcl_total_cloud
pcl_tosaved = pcl.PointCloud()
pcl_tosaved.from_array(pcl_total_cloud)
print pcl_tosaved
if opts.filter_cloud[0] != 0 and opts.filter_cloud[1] != 0 and opts.filter_cloud[2] != 0:
print "doing voxel filtering"
fil = pcl_tosaved.make_voxel_grid_filter()
fil.set_leaf_size(opts.filter_cloud[0], opts.filter_cloud[1], opts.filter_cloud[2])
pcl_tosaved = fil.filter()
print pcl_tosaved
pcl.save(pcl_tosaved, os.path.join(opts.dir_prefix, opts.mapfilename + ".pcd"))
def h5toarray(filename):
hf = h5py.File(filename, 'r')
data = hf.get('data').value
print(data)
pcd_new = p.from_array(data[1])
pcl.save(p, "test.pcd")
import pcl
p = pcl.PointCloud()
cloud_in = pcl.load("hand.ply")
pcl.save(cloud_in, "hand.pcd")
seg = cloud_filtered.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(100)
seg.set_distance_threshold(0.03)
indices, model = seg.segment()
print(model)
cloud_plane = cloud_filtered.extract(indices, negative=False)
# NG : const char* not str
# cloud_plane.to_file('table_scene_mug_stereo_textured_plane.pcd')
pcl.save(cloud_plane, 'table_scene_mug_stereo_textured_plane.pcd')
cloud_cyl = cloud_filtered.extract(indices, negative=True)
seg = cloud_cyl.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_CYLINDER)
seg.set_normal_distance_weight(0.1)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_max_iterations(10000)
seg.set_distance_threshold(0.05)
seg.set_radius_limits(0, 0.1)
indices, model = seg.segment()
print(model)
def testSave(self):
for ext in ["pcd", "ply"]:
d = os.path.join(self.tmpdir, "foo." + ext)
pcl.save(self.p, d)
p = pcl.load_XYZRGBA(d)
self.assertEqual(self.p.size, p.size)
def save_pcd(self, point_cloud, path, _format=False):
# TODO: can load ply file, for the soma mesh
# TODO: should accept multi point_cloud format, currently can only use pcl PointCloud2
pcl.save(point_cloud, path, _format)
def ObjectDetection3D(img_idx):
datatype = 'val' # 'train' #val
KITTI_CLASSES = ('Index0', 'Background', 'Cyclist', 'Car', 'Pedestrian')
class_to_ind = dict(zip(KITTI_CLASSES, range(len(KITTI_CLASSES))))
# Load the Dataset
testset = KITTIDetection(KITTI_ROOT, [datatype], None,
KITTIAnnotationTransform)
testset3D = PartDataset(
root="/home/dllab/kitti_object/data_object_image_2",
image_sets=[datatype],
train=False)
# Necessary Directories
root_dir = "/home/dllab/kitti_object/data_object_image_2"
pcd_dir = "/home/dllab/kitti_object/data_object_velodyne/pcl"
segmented_pcd_dir = "./PCD_Files1"
data_set = "training"
images_dir = os.path.join(root_dir, data_set, "image_{0}".format(2))
detection_dir = './Detections'
calib_dir = '/home/dllab/kitti_object/data_object_velodyne/data_object_calib/training/calib'
box_dir = './bbox_labels_new'
points_dir = "./PCD_Files2/DetectionLocations"
label_dir = os.path.join(root_dir, data_set, "label_{0}".format(2))
network3D = './Pointnet/seg/seg_model_24.pth'
saved_PCD_dir = './Final'
img_real_id = testset.img_id_return(img_idx)
print('indx = {} Image: {}'.format(img_idx, img_real_id))
pcd_id = img_real_id.lstrip('0')
objects_GT = readLabels(label_dir, img_real_id)
# Read the boxes and Points
boxes = read3dBoxes(box_dir, img_real_id)
points = readPoints(points_dir, img_real_id)
# 2D Box Detection
net = build_ssd('test', 300, 4) # initialize SSD
# net.load_state_dict(torch.load(args.weights))
net.load_weights('./ssd_pytorch/weights/ssd300_Resz_KITTI_105000.pth')
image = testset.pull_image(img_idx)
rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
x = cv2.resize(image, (300, 300)).astype(np.float32)
x -= (104.0, 117.0, 123.0)
x = x.astype(np.float32)
x = x[:, :, ::-1].copy()
x = torch.from_numpy(x).permute(2, 0, 1)
xx = Variable(x.unsqueeze(0)) # wrap tensor in Variable
print('XX Size: {}'.format(xx.shape))
if torch.cuda.is_available():
xx = xx.cuda()
y = net(xx)
# plt.figure(figsize=(10,10))
colors = plt.cm.hsv(np.linspace(0, 1, 21)).tolist()
# plt.imshow(rgb_image) # plot the image for matplotlib
currentAxis = plt.gca()
detections = y.data
# scale each detection back up to the image
scale = torch.Tensor(rgb_image.shape[1::-1]).repeat(2)
objs = []
for i in range(detections.size(1)):
j = 0
if i == 2:
limit = 0.5
else:
limit = 0.1
while detections[0, i, j, 0] >= limit:
pt = (detections[0, i, j, 1:] * scale).cpu().numpy()
cv2.rectangle(image, (int(pt[0]), int(pt[1])),
(int(pt[2]), int(pt[3])), COLORS[i % 3], 2)
cv2.putText(image, labelmap[i - 1], (int(pt[0]), int(pt[1])), FONT,
1, (255, 255, 255), 2, cv2.LINE_AA)
objs.append(Object3d_detected(labelmap[i - 1], pt))
j += 1
cv2.imshow('frame', image)
cv2.waitKey(1) & 0xFF
objects = objs
# Get Frustrum
frustrum_points, frustrum_points_total = composeFrustrum(
objects, calib_dir, pcd_dir, img_real_id)
frustrumcloud = pcl.PointCloud(
np.array(frustrum_points_total, dtype=np.float32))
pcl.save(frustrumcloud, "{}/frustrum_{}.pcd".format(saved_PCD_dir, pcd_id))
# Segment Points
# Apply 3D segmentation network
classifier = PointNetDenseCls(k=4)
classifier.load_state_dict(torch.load(network3D))
classifier.eval()
second_index = 0
str_parser = './Final/frustrum_{0}.pcd '.format(pcd_id)
for ind in range(len(frustrum_points)):
obj_points = frustrum_points[ind]
obj_points_num = np.asarray(obj_points)
choice = np.random.choice(len(obj_points_num), 2500, replace=True)
point_set = obj_points_num[choice, :]
point_nn = point_set
point_set = point_set / np.absolute(point_set).max(axis=0)
point = torch.from_numpy(point_set)
point = point.transpose(1, 0).contiguous()
point = Variable(point.view(1, point.size()[0], point.size()[1]))
pred, _ = classifier(point)
pred_choice = pred.data.max(2)[1]
np.set_printoptions(threshold=np.nan)
obj_index = np.nonzero(pred_choice.numpy()[0])
pred_points = point_nn[obj_index]
if pred_points.size != 0:
predcloud = pcl.PointCloud(np.array(pred_points, dtype=np.float32))
pcl.save(
predcloud,
"{}/segmented_{}_{}.pcd".format(saved_PCD_dir, pcd_id, ind))
str_parser = str_parser + \
"{}/segmented_{}_{}.pcd ".format(saved_PCD_dir,
pcd_id, second_index)
second_index = second_index + 1
print(str_parser)
os.system("./Visualizers/showObjects/build/showObjects {} {}".format(
pcd_id, second_index))
# Draw 3D Boxes
os.system("./Visualizers/showBoxes/build/showBoxes {} {}".format(
pcd_id, second_index))
# cloud_cluster->is_dense = true;
#
# std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
# std::stringstream ss;
# ss << "cloud_cluster_" << j << ".pcd";
# writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
# j++;
# }
#
cloud_cluster = pcl.PointCloud()
for j, indices in enumerate(cluster_indices):
# cloudsize = indices
print('indices = ' + str(len(indices)))
# cloudsize = len(indices)
points = np.zeros((len(indices), 3), dtype=np.float32)
# points = np.zeros((cloudsize, 3), dtype=np.float32)
# for indice in range(len(indices)):
for i, indice in enumerate(indices):
# print('dataNum = ' + str(i) + ', data point[x y z]: ' + str(cloud_filtered[indice][0]) + ' ' + str(cloud_filtered[indice][1]) + ' ' + str(cloud_filtered[indice][2]))
# print('PointCloud representing the Cluster: ' + str(cloud_cluster.size) + " data points.")
points[i][0] = cloud_filtered[indice][0]
points[i][1] = cloud_filtered[indice][1]
points[i][2] = cloud_filtered[indice][2]
cloud_cluster.from_array(points)
ss = "cloud_cluster_" + str(j) + ".pcd";
pcl.save(cloud_cluster, ss)
# Removing outliers using a StatisticalOutlierRemoval filter
# http://pointclouds.org/documentation/tutorials/statistical_outlier.php#statistical-outlier-removal
import pcl
p = pcl.load("table_scene_lms400.pcd")
fil = p.make_statistical_outlier_filter()
fil.set_mean_k(50)
fil.set_std_dev_mul_thresh(1.0)
pcl.save(fil.filter(), "table_scene_lms400_inliers.pcd")
fil.set_negative(True)
pcl.save(fil.filter(), "table_scene_lms400_outliers.pcd")
# clipper.filter(*outcloud);
tx = 0
ty = 0
tz = 0
clipper.set_Translation(tx, ty, tz)
rx = 0
ry = 0
rz = 0
clipper.set_Rotation(rx, ry, rz)
minx = -1.5
miny = -1.5
minz = 2
mins = 0
maxx = 3.5
maxy = 3.5
maxz = 3
maxs = 0
clipper.set_MinMax(minx, miny, minz, mins, maxx, maxy, maxz, maxs)
clipper.Filtering(outcloud)
pcl.save(outcloud, "test.pcd")
# stringstream outfilename;
# outfilename << outfile << tracklet->objectType << i << ".pcd";
# if(!outcloud->empty()){
# cout << "Found "<<outcloud->size() << " points, writing to " << outfilename.str() << endl;
# writer.write<PointXYZI> (outfilename.str(), *outcloud, false);
# }else{
# cerr << "Couldn't find points for tracklet" << tracklet->objectType << i << endl;
# }
def main():
# // Read in the cloud data
# pcl::PCDReader reader;
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
# reader.read ("table_scene_lms400.pcd", *cloud);
# std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
cloud = pcl.load(
'/python-pcl/examples/pcldata/tutorials/table_scene_lms400.pcd')
# // Create the filtering object: downsample the dataset using a leaf size of 1cm
# pcl::VoxelGrid<pcl::PointXYZ> vg;
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
# vg.setInputCloud (cloud);
# vg.setLeafSize (0.01f, 0.01f, 0.01f);
# vg.filter (*cloud_filtered);
# std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
vg = cloud.make_voxel_grid_filter()
vg.set_leaf_size(0.0029, 0.0029, 0.0029)
cloud_filtered = vg.filter()
# // Create the segmentation object for the planar model and set all the parameters
# pcl::SACSegmentation<pcl::PointXYZ> seg;
# pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
# pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
# pcl::PCDWriter writer;
# seg.setOptimizeCoefficients (true);
# seg.setModelType (pcl::SACMODEL_PLANE);
# seg.setMethodType (pcl::SAC_RANSAC);
# seg.setMaxIterations (100);
# seg.setDistanceThreshold (0.02);
seg = cloud.make_segmenter()
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_MaxIterations(100)
seg.set_distance_threshold(0.02)
# int i=0, nr_points = (int) cloud_filtered->points.size ();
# while (cloud_filtered->points.size () > 0.3 * nr_points)
# {
# // Segment the largest planar component from the remaining cloud
# seg.setInputCloud (cloud_filtered);
# seg.segment (*inliers, *coefficients);
# if (inliers->indices.size () == 0)
# {
# std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
# break;
# }
# // Extract the planar inliers from the input cloud
# pcl::ExtractIndices<pcl::PointXYZ> extract;
# extract.setInputCloud (cloud_filtered);
# extract.setIndices (inliers);
# extract.setNegative (false);
#
# // Get the points associated with the planar surface
# extract.filter (*cloud_plane);
# std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
#
# // Remove the planar inliers, extract the rest
# extract.setNegative (true);
# extract.filter (*cloud_f);
# *cloud_filtered = *cloud_f;
# }
i = 0
nr_points = cloud_filtered.size
# while nr_points > 0.3 * nr_points:
# # Segment the largest planar component from the remaining cloud
# [inliers, coefficients] = seg.segment()
# # extract = cloud_filtered.extract()
# # extract = pcl.PointIndices()
# cloud_filtered.extract(extract)
# extract.set_Indices (inliers)
# extract.set_Negative (false)
# cloud_plane = extract.filter ()
#
# extract.set_Negative (True)
# cloud_f = extract.filter ()
# cloud_filtered = cloud_f
# Creating the KdTree object for the search method of the extraction
# pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
# tree->setInputCloud (cloud_filtered);
tree = cloud_filtered.make_kdtree()
# tree = cloud_filtered.make_kdtree_flann()
# std::vector<pcl::PointIndices> cluster_indices;
# pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
# ec.setClusterTolerance (0.02); // 2cm
# ec.setMinClusterSize (100);
# ec.setMaxClusterSize (25000);
# ec.setSearchMethod (tree);
# ec.setInputCloud (cloud_filtered);
# ec.extract (cluster_indices);
ec = cloud_filtered.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.03)
ec.set_MinClusterSize(200)
ec.set_MaxClusterSize(5000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
print('cluster_indices : ' + str(len(cluster_indices)) + " count.")
# print('cluster_indices : ' + str(cluster_indices.indices.max_size) + " count.")
# int j = 0;
# for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
# {
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
# for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
# cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
# cloud_cluster->width = cloud_cluster->points.size ();
# cloud_cluster->height = 1;
# cloud_cluster->is_dense = true;
#
# std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
# std::stringstream ss;
# ss << "cloud_cluster_" << j << ".pcd";
# writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
# j++;
# }
#
cloud_cluster = pcl.PointCloud()
for j, indices in enumerate(cluster_indices):
# cloudsize = indices
print('indices = ' + str(len(indices)))
# cloudsize = len(indices)
# points = np.zeros((len(indices), 3), dtype=np.float32)
# points = np.zeros((cloudsize, 3), dtype=np.float32)
cloud_output = []
# for indice in range(len(indices)):
for i, indice in enumerate(indices):
# print('dataNum = ' + str(i) + ', data point[x y z]: ' + str(cloud_filtered[indice][0]) + ' ' + str(cloud_filtered[indice][1]) + ' ' + str(cloud_filtered[indice][2]))
# print('PointCloud representing the Cluster: ' + str(cloud_cluster.size) + " data points.")
# points[i][0] = cloud_filtered[indice][0]
# points[i][1] = cloud_filtered[indice][1]
# points[i][2] = cloud_filtered[indice][2]
cloud_output.append([
cloud_filtered[indice][0], cloud_filtered[indice][1],
cloud_filtered[indice][2]
])
cloud_cluster.from_list(cloud_output)
ss = "cloud_cluster_" + str(j) + ".pcd"
pcl.save(cloud_cluster, ss)
# RANSAC plane segmentation
# Extract inliers
# Save pcd for table
# pcl.save(cloud, filename)
# Extract outliers
# Save pcd for tabletop objects
<<<<<<< HEAD
=======
# Extract inliers
extracted_inliers = cloud_filtered.extract(inliers, negative=False)
filename_in = 'extracted_inliers.pcd'
extracted_outliers = cloud_filtered.extract(inliers, negative=True)
filename_out = 'extracted_outliers.pcd'
# Save pcd for table
pcl.save(extracted_inliers, filename_in)
pcl.save(extracted_outliers, filename_out)
print('success!')
>>>>>>> a9ce4b9ef214ad299f7141e6dbc2e1709ef5d2af
def evaluate(model_path, out_filename, booth_data, min_list):
'''
:param model_path: trained model path
:param filepath: file stored dir
:param booth_data: point cloud data type : Array / value [x, y, z, r, g, b, label(5)]
:param xyz_min: min xyz of ply data(all)
:param min_list: min xyz of each npy data
:return: 3D model path, bounding box area of chair data [max point(x, y, z), min point(x, y, z]
'''
is_training = False
# BATCH_SIZE = 1
# NUM_POINT = 4096
# GPU_INDEX = 0
MODEL_PATH = model_path
DUMP_DIR = os.path.join(os.path.dirname(out_filename), 'dump')
if not os.path.exists(DUMP_DIR): os.mkdir(DUMP_DIR)
# ROOM_PATH_LIST = [os.path.join(ROOT_DIR, line.rstrip()) for line in open(room_data_filelist)]
with tf.device('/gpu:' + str(GPU_INDEX)):
pointclouds_pl, labels_pl = placeholder_inputs(BATCH_SIZE, NUM_POINT)
is_training_pl = tf.placeholder(tf.bool, shape=())
# simple model
pred = get_model(pointclouds_pl, is_training_pl)
loss = get_loss(pred, labels_pl)
pred_softmax = tf.nn.softmax(pred)
# Add ops to save and restore all the variables.
saver = tf.train.Saver()
# Create a session
config = tf.ConfigProto()
config.gpu_options.allow_growth = True
config.allow_soft_placement = True
config.log_device_placement = True
sess = tf.Session(config=config)
# Restore variables from disk.
saver.restore(sess, MODEL_PATH)
ops = {'pointclouds_pl': pointclouds_pl,
'labels_pl': labels_pl,
'is_training_pl': is_training_pl,
'pred': pred,
'pred_softmax': pred_softmax,
'loss': loss}
all_ceiling = list()
all_floor = list()
all_wall = list()
all_chair = list()
all_table = list()
all_clutter = list()
# scaned_data = np.load(ROOM_PATH_LIST[0])
for each_i in range(len(booth_data)):
ceiling_list, floor_list, wall_list, chair_list, table_list, clutter_list = eval_one_epoch(sess, ops, booth_data[each_i])
if len(ceiling_list) != 0:
temp_value_1 = np.asarray(ceiling_list)
temp_value_1 += min_list[each_i]
ceiling_cloud = pcl.PointCloud()
ceiling_cloud.from_list(temp_value_1.tolist())
pcl.save(ceiling_cloud,
os.path.join(DUMP_DIR, os.path.basename(out_filename)) + '_ceiling' + "_" + str(each_i)+".pcd")
if len(floor_list) != 0:
temp_value_2 = np.asarray(floor_list)
temp_value_2 += min_list[each_i]
floor_cloud = pcl.PointCloud()
floor_cloud.from_list(temp_value_2.tolist())
pcl.save(floor_cloud,
os.path.join(DUMP_DIR, os.path.basename(out_filename)) + '_floor' + "_" + str(each_i)+".pcd")
if len(wall_list) != 0:
temp_value_3 = np.asarray(wall_list)
temp_value_3 += min_list[each_i]
wall_cloud = pcl.PointCloud()
wall_cloud.from_list(temp_value_3.tolist())
pcl.save(wall_cloud, os.path.join(DUMP_DIR, os.path.basename(out_filename)) + '_wall' + "_" + str(each_i)+".pcd")
all_wall += temp_value_3.tolist()
if len(chair_list) != 0:
temp_value_4 = np.asarray(chair_list)
temp_value_4 += min_list[each_i]
chair_cloud = pcl.PointCloud()
chair_cloud.from_list(temp_value_4.tolist())
pcl.save(chair_cloud,
os.path.join(DUMP_DIR, os.path.basename(out_filename)) +'_chair' + "_" + str(each_i)+".pcd")
all_chair += temp_value_4.tolist()
if len(table_list) != 0:
temp_value_5 = np.asarray(table_list)
temp_value_5 += min_list[each_i]
table_cloud = pcl.PointCloud()
table_cloud.from_list(temp_value_5.tolist())
pcl.save(table_cloud, os.path.join(DUMP_DIR, os.path.basename(out_filename)) + '_table' + "_" + str(each_i)+".pcd")
all_table += temp_value_5.tolist()
if len(clutter_list) != 0:
temp_value_6 = np.asarray(wall_list)
temp_value_6 += min_list[each_i]
clutter_cloud = pcl.PointCloud()
clutter_cloud.from_list(temp_value_6.tolist())
all_wall += temp_value_6.tolist()
pcl.save(clutter_cloud,
os.path.join(DUMP_DIR, os.path.basename(out_filename)) + "_clutter" + "_" + str(each_i)+".pcd")
print "chair : ", len(all_chair)
''' Third Process '''
dae_filename = os.path.join(DUMP_DIR, os.path.basename(out_filename))
dae_filename = dae_filename + '.dae'
wall_cloud = pcl.PointCloud()
wall_cloud.from_list(all_wall)
wall_surface_list = make_wall_info(wall_cloud)
print wall_surface_list
# chair_cloud = pcl.PointCloud()
# chair_cloud.from_list(all_chair)
# chair_boundary = make_chair_info(chair_cloud, dae_filename)
print dae_filename
def capture_image(self, full_data):
if self.flag and full_data[
2].size > 0 and self.object_counter < self.max_objects:
self.time_stamp = time.time()
print("inside save", self.object_counter + 1)
if self.toggle_switch.text() == "Stop":
self.object_counter += 1
# else:
# self.toggle_switch.setText("Start")
# self.toggle_switch.setStyleSheet("background-color: green")
# self.flag = False
# print(,"point cloud data")
self._image_counter[self.label_list.index(
str(self.label_box.currentText()))] += 1
name = str(self.label_box.currentText()) + "_{}.png".format(
self.time_stamp)
annotation_name = str(
self.label_box.currentText()) + "_{}.xml".format(
self.time_stamp)
pointcloud_name = str(
self.label_box.currentText()) + "_{}.pcd".format(
self.time_stamp)
frame_name = str(self.label_box.currentText()) + "_{}".format(
self.time_stamp)
rgb_img = full_data[0][:, :640]
mask_img = full_data[0][:, 640:]
bbox_coordinates = full_data[1]
obj_pixels = np.where(mask_img == 255)
mask_img[obj_pixels] = self.label_list.index(
str(self.label_box.currentText())) + 1
if self.save_rgb:
cv2.imwrite(self.generator_options.get_image_path() + name,
rgb_img)
if self.save_semantic_label:
cv2.imwrite(self.generator_options.get_label_path() + name,
mask_img)
if self.save_bbox:
writer = Writer(self.generator_options.get_image_path() + name,
rgb_img.shape[0], rgb_img.shape[1])
writer.addObject(str(self.label_box.currentText()),
bbox_coordinates[0], bbox_coordinates[2],
bbox_coordinates[2], bbox_coordinates[3])
writer.save(self.save_folder_path +
"/captured_data/obj_det_label/" + annotation_name)
if self.save_pointcloud:
pcl.save(
full_data[2], self.save_folder_path +
"/captured_data/pointclouds/" + pointcloud_name)
if self.save_depth:
# print(full_data[3].get_frame_number())
# adds frame numer at the end of filename yet to solve the issue
# saver = rs.save_single_frameset(filename=self.save_folder_path+"/captured_data/depth_frames/"+frame_name)
# saver.process(full_data[3])
cv2.imwrite(
self.save_folder_path + "/captured_data/depth_frames/" +
name, full_data[3])
self.flag = False
elif self.object_counter == self.max_objects:
# print("inside else loop")
self.toggle_switch_status()
def pcl_callback(pcl_msg):
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()
# Choose a voxel (also known as 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()
filename = 'voxel_downsampled.pcd'
pcl.save(cloud_filtered, filename)
# TODO: 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.
cloud_filtered = passthrough.filter()
filename = 'pass_through_filtered.pcd'
pcl.save(cloud_filtered, filename)
# TODO: 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
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# Call the segment funcion to obtain set of inlier indices
# and model coefficients
inliers, coefficients = seg.segment()
# TODO: Extract inliers and outliers
# Extract inliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
filename = 'cloud_table.pcd'
pcl.save(cloud_table, filename)
# Extract outliers
cloud_objects = cloud_filtered.extract(inliers, negative=True)
filename = 'cloud_objects.pcd'
pcl.save(cloud_objects, filename)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(
cloud_objects) # Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
# NOTE: These are poor choices of clustering parameters
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.01)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(2000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract(
) #cluster_indices ahora contiene una lista de los objetos
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
#Assign a color corresponding to each segmented object in scene
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
white_cloud[indice][0], white_cloud[indice][1],
white_cloud[indice][2],
rgb_to_float(cluster_color[j])
])
#Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cloud_cluster = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_mask_pub.publish(ros_cloud_cluster)
def test_write_cloud2():
a = np.random.randn(100, 3).astype(np.float32)
p1 = pcl.PointCloud(a)
pcl.save(p1, "temppcl.pcd")
resolution=0.2)
print('processsing {} in {}'.format(file_name, fold))
# print P.shape
# print voxel.shape,scale_points.shape
all_cnt += steps #all_cnt+=1
# save pointcloud from bin
# if P.shape[0]>0:
# cloud=pcl.PointCloud(P) # P should be type: float
# # pcl.save(cloud,'./pcd/{}.pcd'.format(file_name.split('.bin')[0]))
# #pcl.save(cloud,'./pcd/{}.pcd'.format(file_name.split('.bin')[0]))
# save filterred pointcloud and voxel
idx = 0
for voxel, scale_points in zip(voxel_list, scale_points_list):
idx += 1
if scale_points.shape[0] > 0:
success_cnt += 1
cloud = pcl.PointCloud(scale_points.astype(np.float32))
pcl.save(
cloud, './pcd_voxel2_r2/{}_{}.pcd'.format(
file_name.split('.bin')[0], idx))
np.save(
'./voxel_npy_eval_r2/{}_{}.npy'.format(
file_name.split('.bin')[0], idx), voxel)
else:
print('processed {} is empty'.format(file_name))
print('process done :{}/{} success, {} failed'.format(
success_cnt, all_cnt, all_cnt - success_cnt))
sys.exit()
if input_dir[-1] == '/':
index = input_dir[:-2].rfind('/')
else:
index = input_dir.rfind('/')
output_dir = input_dir[:index]
if transfer_type == 0:
output_dir = os.path.join(output_dir, "Binary_pcd")
else:
output_dir = os.path.join(output_dir, "Ascii_pcd")
if os.path.exists(output_dir) == False:
os.makedirs(output_dir)
print("Start to tranfer!")
print("Output will save to ", output_dir)
for filePath in os.listdir(input_dir):
in_pcd_file_path = os.path.join(input_dir, filePath)
out_pcd_file_path = os.path.join(output_dir, filePath)
if in_pcd_file_path.split('.')[-1] != 'pcd':
print("Error format file: ", filePath)
print("Skip this file!")
continue
pt = pcl.load_XYZI(in_pcd_file_path, 'pcd')
if transfer_type == 0:
pcl.save(pt, out_pcd_file_path, 'pcd', True)
else:
pcl.save(pt, out_pcd_file_path, 'pcd', False)
print("Done!")
if os.path.isdir(input_dir + '/' + folder):
videos.append(folder)
videos.sort()
w = 640
h = 480
_, category_id2name = generate_categories()
# Process for each video
for video in videos:
instances = [[] for i in range(22)
] # 21 classes, for the class id can be index directly
generate_points(video)
video_output_dir = output_dir + '/' + video
if not os.path.exists(video_output_dir):
os.makedirs(video_output_dir)
for i in range(len(instances)):
if len(instances[i]) > 0:
all_points = []
for instance in instances[i]:
all_points = all_points + instance
t_points = np.array(all_points, dtype=np.float32)
p = pcl.PointCloud(t_points)
pcl.save(p,
output_dir + '/' + video + '/' +
category_id2name[str(i)] + ".pcd",
format='pcd')
while (True):
cloud_path, cloud_format = get_file()
# for path in cloud_path:
folder_name = cloud_path[-13:-4]
cluster_index = 0
cluster_name = str(cluster_index)[-4:]
print("Cluster_" + str(cluster_index)[-4:] + "." + cloud_format)
pcl_cloud = pcl.load(cloud_path, cloud_format)
# for i in range(400):
clusters = segment(pcl_cloud, 2.75, min_cluster=5, view=False)
for cluster in clusters:
# pcl.save(cluster, ("./data/PRICT-28/clusters/Cluster_" + str(cluster_index)[-4:] + "." + cloud_format),
# format=cloud_format)
try:
os.mkdir("F:/Data/Raccoon/Clean/Clustered/Right/81661206" +
folder_name)
except:
pass
pcl.save(cluster,
("F:/Data/Raccoon/Clean/Clustered/Right/81661206" +
folder_name + "/Cluster_" + str(cluster_index)[-4:] +
"." + cloud_format),
format=cloud_format)
print("Saving ", end="")
print("Cluster_" + str(cluster_index)[-4:] + "." + cloud_format)
cluster_index += 1
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)))
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]
#print xy2
return xy2
def main(cfg):
instrinsic = []
with open(os.path.join(cfg.intrinsic_seg), 'r') as f:
while True:
line = f.readline()
if not line:
break
eles = line.split()
for ele in eles:
instrinsic.append(ele)
instrinsic = np.array(instrinsic, dtype=np.float32).reshape([4, 4])
instrinsic_inv = np.linalg.inv(instrinsic)
depth_files = os.listdir(cfg.inputdepthdir)
depth_files.sort()
seg_files = os.listdir(cfg.inputsegdir)
seg_files.sort()
for depth_file, seg_file in zip(depth_files, seg_files):
trajectory = []
with open(os.path.join(cfg.trajectorydir, depth_file[:-4]+'.txt'), 'r') as ft:
while True:
line = ft.readline()
if not line:
break
eles = line.split()
for ele in eles:
trajectory.append(ele)
trajectory = np.array(trajectory, dtype=np.float32).reshape([4, 4])
depth = cv2.imread(os.path.join(cfg.inputdepthdir, depth_file), -1)
seg = cv2.imread(os.path.join(cfg.inputsegdir, seg_file))
cloud = pcl.PointCloud_PointXYZRGBA()
rows = seg.shape[0]
cols = seg.shape[1]
depth = cv2.resize(depth, (cols, rows))
points = np.zeros([rows*cols, 4])
camera = np.zeros([4, 1])
for i in range(rows):
for j in range(cols):
df = depth[i, j]
if df == 0:
pass
else:
camera[0, 0] = df*j
camera[1, 0] = df*i
camera[2, 0] = df
camera[3, 0] = 1
camera_cor = np.dot(instrinsic_inv, camera)
world = np.dot(trajectory, camera_cor)
x = np.int((world[0, 0]))
y = np.int((world[1, 0]))
z = np.int((world[2, 0]))
r = (0xff & seg[i, j, 2]) << 16
g = (0xff & seg[i, j, 1]) << 8
b = (0xff & seg[i, j, 0])
rgb = r | g | b
# print(rgb)
points[i*rows+j, :] = [x, y, z, rgb]
points = np.array(points, dtype=np.float32)
cloud.from_array(points)
pcl.save(cloud, os.path.join(cfg.saveplydir, depth_file[:-4]+'.ply'), format='ply')
tree = cloud_filtered.make_kdtree()
ec = cloud_filtered.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(25000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
print('cluster_indices : ' + str(cluster_indices.count) + " count.")
cloud_cluster = pcl.PointCloud()
for j, indices in enumerate(cluster_indices):
# cloudsize = indices
print('indices = ' + str(len(indices)))
# cloudsize = len(indices)
points = np.zeros((len(indices), 3), dtype=np.float32)
# points = np.zeros((cloudsize, 3), dtype=np.float32)
# for indice in range(len(indices)):
for i, indice in enumerate(indices):
# print('dataNum = ' + str(i) + ', data point[x y z]: ' + str(cloud_filtered[indice][0]) + ' ' + str(cloud_filtered[indice][1]) + ' ' + str(cloud_filtered[indice][2]))
# print('PointCloud representing the Cluster: ' + str(cloud_cluster.size) + " data points.")
points[i][0] = cloud_filtered[indice][0]
points[i][1] = cloud_filtered[indice][1]
points[i][2] = cloud_filtered[indice][2]
cloud_cluster.from_array(points)
ss = "cloud_cluster_" + str(j) + ".pcd"
pcl.save(cloud_cluster, ss)
# Downsampling time
dt = time.time() - now
# Debug
if IS_DEBUG:
# Print the size of the original point cloud
print("The size of the original pointcloud: ", oriCloud.size)
# Print process time
print("Downsampleing time: %10.9f", dt)
# Print the size of the downsampled point cloud
print("The size of the downsampled pointcloud: ", vgCloud.size)
# Save the image for visualization
if SAVE_IMAGE:
pcl.save(oriCloud, "oriCloud.pcd")
pcl.save(vgCloud, "vgCloud.pcd")
###############################
# Apply Passthrough Filter #
# (crop the image) #
###############################
# Starting time for downsampling
now1 = time.time()
# Create a passthrough filter object
passthrough = vgCloud.make_passthrough_filter()
# Assign axis and range to the passthrough filter()
FILTER_AXIS = 'z'
# std::cerr << "PointCloud after projection has: "
# << cloud_projected->points.size () << " data points." << std::endl;
proj = cloud_filtered.make_ProjectInliers()
proj.set_model_type (pcl.SACMODEL_PLANE);
# proj.setIndices (inliers);
# proj.setModelCoefficients (coefficients)
cloud_projected = proj.filter ()
print ('PointCloud after projection has: ' + str(cloud_projected.size) + ' data points.')
# // Create a Concave Hull representation of the projected inliers
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
# pcl::ConcaveHull<pcl::PointXYZ> chull;
# chull.setInputCloud (cloud_projected);
# chull.setAlpha (0.1);
# chull.reconstruct (*cloud_hull);
# std::cerr << "Concave hull has: " << cloud_hull->points.size ()
# << " data points." << std::endl;
# cloud_projected = pcl.PointCloud()
chull = cloud_projected.make_ConcaveHull()
chull.set_Alpha (0.1)
cloud_hull = chull.reconstruct ()
print ('Concave hull has: ' + str(cloud_hull.size) + ' data points.')
# pcl::PCDWriter writer;
# writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);
if cloud_hull.size != 0:
pcl.save(cloud_hull, 'table_scene_mug_stereo_textured_hull.pcd')
# save pre-process pointcloud voxel
idx = 0
if not os.path.exists(save_dir):
os.makedirs(save_dir, exist_ok=True)
for points in cloud_list:
voxels, inside_points = \
helper.voxelize(points, voxel_size=(24,24,24), padding_size=(32,32,32), resolution=0.1)
if FLAGS.viz:
viewer.plot3DVoxel(voxels)
# save pointcloud to *.pcd
if FLAGS.pcd:
if inside_points.shape[0] > 0:
pc = pcl.PointCloud(points)
pcd_name = '{}/{}_{}.pcd'.format(
save_dir,
file_name.split('.bin')[0], idx)
pcl.save(pc, pcd_name)
print('saved pcd. {}'.format(pcd_name))
if inside_points.shape[0] > 0:
save_name = '{}/{}_{}.npy'.format(
save_dir,
file_name.split('.bin')[0], idx)
np.save(save_name, voxels)
print('saved npy. {}'.format(save_name))
idx += 1
for ind in range(0,points_array.shape[0]):
y = np.matrix([[points_array[ind][0]],[points_array[ind][1]],[points_array[ind][2]],[1.0]]);
Tr_y = Tr_velo_to_cam*y
if Tr_y[2] > 0:
X = P2 * R0_rect * Tr_y
#FOR LOOP FOR ALL OBJECTS
for index in range(0,len(objects)):
obj=objects[index]
if ( ( obj.xmin < X[0]/X[2] < obj.xmax ) and ( obj.ymin < X[1]/X[2] < obj.ymax ) ):
#print(X)
filtered_points_array.append(points_array[ind])
# check boxes from training, not the groundtruth and do it
# print(Tr_y)
# for x in np.nditer(a):
# print(x)
# 2d equal ret times ba ba times tr velo times y
#outcloud = pcl.PointCloud(np.array([[1, 2, 3], [3, 4, 5],[6,7,8]], dtype=np.float32))
outcloud = pcl.PointCloud(np.array(filtered_points_array, dtype=np.float32))
#DONT CHANGE NAMES BECAUSE OF CPP CODE
pcl.save(outcloud, "./PCD_Files/segmented_{}.pcd".format(img_idx))
print('Cloud saved')
os.system("/home/emeka/Schreibtisch/AIS/deleteme/build/test_pcl {}".format(img_idx))
#port of
#http://pointclouds.org/documentation/tutorials/statistical_outlier.php
#you need to download
import pcl
p = pcl.load("table_scene_lms400.pcd")
fil = p.make_statistical_outlier_filter()
fil.set_mean_k(50)
fil.set_std_dev_mul_thresh(1.0)
pcl.save(fil.filter(), "table_scene_lms400_inliers.pcd")
fil.set_negative(True)
pcl.save(fil.filter(), "table_scene_lms400_outliers.pcd")
# a voxel are assigned to that voxel and are statistically combined into # one output point. # Create a Voxel Grid filter object for the input point cloud vox = cloud.make_voxel_grid_filter() # Choose a voxel size (also known as leaf size) of n cubic meters per voxel LEAF_SIZE = 0.01 # Set the voxel (or leaf) size; can adjust size along each dimension 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 # Allows a 3D point cloud to be cropped by specifying an axis with cut-off # values along that axis. The region allowed to 'pass through' is often # called the 'region of interest.' # Create a pass through filter objeect 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
# Import PCL module
import pcl
# Load Point Cloud file
cloud = pcl.load_XYZRGB('tabletop.pcd')
pcl.save(cloud, 'cloud.pcd')
# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()
# Choose a voxel (also known as 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()
pcl.save(cloud_filtered, 'voxel_downsampled.pcd')
# 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.76
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
import octomap
import numpy as np
p = pcl.load('/home/ros/TestM/Meowth.nvm.cmvs/Meowth.0.ply','ply')
print(p)
pcl.save(p,'/home/ros/TestM/Meowth.nvm.cmvs/Meowth.pcd','pcd')
p = pcl.load('/home/ros/TestM/Meowth.nvm.cmvs/Meowth.pcd')
fil = p.make_statistical_outlier_filter()
fil.set_mean_k (50)
fil.set_std_dev_mul_thresh (1.0)
fil.filter().to_file("/home/ros/TestM/Meowth.nvm.cmvs/inliersSparse.pcd")
seg = cloud_filtered.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(100)
seg.set_distance_threshold(0.03)
indices, model = seg.segment()
print(model)
cloud_plane = cloud_filtered.extract(indices, negative=False)
# NG : const char* not str
# cloud_plane.to_file('table_scene_mug_stereo_textured_plane.pcd')
pcl.save(cloud_plane, 'table_scene_mug_stereo_textured_plane.pcd')
cloud_cyl = cloud_filtered.extract(indices, negative=True)
seg = cloud_cyl.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_CYLINDER)
seg.set_normal_distance_weight(0.1)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_max_iterations(10000)
seg.set_distance_threshold(0.05)
seg.set_radius_limits(0, 0.1)
indices, model = seg.segment()
print(model)
if count_velo == count_odom and count_velo == count_nodes + 1 and count_odom == count_nodes + 1:
count_nodes = count_nodes + 1
print "saving:", count_nodes
# first built map, so surely increasing no of nodes, no loop close yet
# if not graph: #Empty
if not len(graph.nodes): # Empty
this_new_node = GraphNode()
this_new_node.node_name = str(count_nodes)
this_new_node.pose = last_pose
# graph[count_nodes] = [last_pose, []]
# Ok we keep dict to fasten back reference later when building map
# Ok to keep ordering (to refer back to rosified msg list), we add counter to the dict :)
# graph_dict[count_nodes] = [last_pose, [], count_nodes-1]
graph.nodes.append(this_new_node)
if not opts.nocloudsave:
pcl.save(pcl_cloud, os.path.join(opts.dir_prefix, "scan" + format(count_nodes, "05d") + ".pcd"))
cloud_T = np.ones((len(last_cloud_array), 4))
cloud_T[:, :-1] = pcl_cloud
cloud_T = np.dot(cloud_T, inv_hom_mat.T)
cloud_T = cloud_T[:, :-1]
pcl_cloud_T = pcl.PointCloud()
pcl_cloud_T.from_array(np.array(cloud_T, dtype=np.float32))
pcl.save(pcl_cloud_T, os.path.join(opts.dir_prefix, "scanorg" + format(count_nodes, "05d") + ".pcd"))
if first_flag:
first_flag = False
else:
# This should not happen
raise Exception("graph still empty but this is not first initialization, quiting check!")
exit()
else:
# if count_nodes-1 in graph_dict:
def window_line_cp(preds, gts, averages, info_path, save_path, nid):
"""
generate cloud points for detected window line, in local system instead in global
"""
if preds is None or gts is None:
return
with open(info_path, 'rb') as f:
info = pickle.load(f)
#ref_plane = info['ref_plane']
buttom_edge = info['buttom_edge']
left_edge = info['left_edge']
Trans = info['trans_i2o']
rows, cols = averages.shape
overlap = bbox_overlaps(preds, gts)
idx_assigned_gt = overlap.argmax(axis=1)
confidence = overlap.max(axis=1)
assigned_gts = gts[idx_assigned_gt]
flag = np.where(confidence >= config.iou_thres, 1, 0)
if np.sum(flag) > 0:
idx_tp = flag.nonzero()[0]
tps = preds[idx_tp]
else:
tps = []
flag = np.where(confidence < config.iou_thres, 1, 0)
if np.sum(flag) > 0:
idx_fp = flag.nonzero()[0]
fps = preds[idx_fp]
else:
fps = []
tp_points = list()
for e in tps:
x0, y0, x1, y1 = e
x = np.arange(x0, x1, 0.01, dtype=np.float32) * 0.02 + left_edge
z = (np.ones(x.shape, dtype=np.float32) *
(rows - y1)) * 0.02 + buttom_edge
y = (np.ones(x.shape, dtype=np.float32) * averages[(y1 + y0) // 2,
(x0 + x1) // 2])
points = np.zeros((len(x), 3), dtype=np.float32)
points[:, 0] = x
points[:, 1] = y
points[:, 2] = z
p2 = points.copy()
p2[:, 2] += 0.01
p3 = points.copy()
p3[:, 1] += 0.01
#points = np.concatenate([x, y, z], axis=1)
tp_points.append(points)
tp_points.append(p2)
tp_points.append(p3)
if len(tp_points) == 0:
tp_points = None
else:
tp_points = np.concatenate(tp_points, axis=0)
fp_points = list()
for e in fps:
x0, y0, x1, y1 = e
x = np.arange(x0, x1, 0.01, dtype=np.float32) * 0.02 + left_edge
z = (np.ones(x.shape, dtype=np.float32) *
(rows - y1)) * 0.02 + buttom_edge
y = (np.ones(x.shape, dtype=np.float32) * averages[(y1 + y0) // 2,
(x0 + x1) // 2])
points = np.zeros((len(x), 3), dtype=np.float32)
points[:, 0] = x.copy()
points[:, 1] = y.copy()
points[:, 2] = z.copy()
#points = np.concatenate([x, y, z], axis=1)
p2 = points.copy()
p2[:, 2] += 0.01
p3 = points.copy()
p3[:, 1] += 0.01
#points = np.concatenate([x, y, z], axis=1)
fp_points.append(points)
fp_points.append(p2)
fp_points.append(p3)
if len(fp_points) == 0:
fp_points = None
else:
fp_points = np.concatenate(fp_points, axis=0)
basex = info['original_x']
basey = info['original_y']
basez = info['original_z']
cloud = pcl.PointCloud()
if tp_points is not None:
tp_points_aug = np.ones((len(tp_points), 4), dtype=np.float32)
tp_points_aug[:, :3] = tp_points
o_tp_points = Trans @ tp_points_aug.T
o_tp_points = o_tp_points.T[:, 0:3]
'''
o_tp_points[:, 0] += basex
o_tp_points[:, 1] += basey
o_tp_points[:, 2] += basez
'''
cloud.from_array(o_tp_points)
pcl.save(cloud,
os.path.join(save_path, '{}_tp.ply'.format(nid)),
format="ply")
if fp_points is not None:
fp_points_aug = np.ones((len(fp_points), 4), dtype=np.float32)
fp_points_aug[:, :3] = fp_points
o_fp_points = Trans @ fp_points_aug.T
o_fp_points = o_fp_points.T[:, 0:3]
'''
o_fp_points[:, 0] += basex
o_fp_points[:, 1] += basey
o_fp_points[:, 2] += basez
'''
cloud.from_array(o_fp_points)
pcl.save(cloud,
os.path.join(save_path, '{}_fp.ply'.format(nid)),
format="ply")
# 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)
def run(self):
'''
Runs SfM on the images.
'''
instance1, instance2 = self.instances
# Initial report (spit back params)
print ''
print 'Using images {} and {}'.format(instance1.id, instance2.id)
print ''
print 'SIFT matching ratio : {}'.format(sfm.SIFT_MATCHING_RATIO)
print 'Window radius : {}'.format(sfm.WINDOW_RADIUS)
print 'Skip factor : {}'.format(sfm.CORRESPONDENCE_SKIP)
print 'Filtering point cloud : {}'.format(sfm.FILTER_POINT_CLOUD)
print ''
# Find features
msg = 'Finding features...'
Util.print_progress_bar(14, msg)
instance1.find_features()
Util.print_progress_bar(28, msg)
instance2.find_features()
if sfm.SHOW_FEATURES:
self.DEBUG_show_features()
# Match features
msg = 'Matching...'
Util.print_progress_bar(43, msg)
instance1.match_features(instance2)
if sfm.SHOW_MATCHES:
self.DEBUG_show_matches()
if sfm.SHOW_EPILINES:
self.DEBUG_show_epilines()
# Find correspondences
msg = 'Triangulating...'
Util.print_progress_bar(57, msg)
points, depth = instance1.triangulate_points(instance2)
# Names for output files
name = '{}-{}-r{}s{:02}w{:02}'.format(instance1.id, instance2.id,
sfm.SIFT_MATCHING_RATIO, sfm.CORRESPONDENCE_SKIP,
sfm.WINDOW_RADIUS)
grey_fig_name = self.output_dir+name+'-grey.pdf'
color_fig_name = self.output_dir+name+'-color.pdf'
ptcld_name = self.output_dir+name+'.ply'
# Save depth map
msg = 'Depth map...'
Util.print_progress_bar(71, msg)
plt.imshow(depth, cmap=plt.cm.bone)
plt.savefig(grey_fig_name)
plt.imshow(depth)
plt.savefig(color_fig_name)
if sfm.SHOW_DEPTH_MAP:
self.DEBUG_show_depth_map(plt)
# Create and save point cloud
msg = 'Point cloud...'
Util.print_progress_bar(86, msg)
ptcld = pcl.PointCloud(np.float32(points))
if sfm.FILTER_POINT_CLOUD:
fil = ptcld.make_statistical_outlier_filter()
fil.set_mean_k(50)
fil.set_std_dev_mul_thresh(1.0)
ptcld = fil.filter()
pcl.save(ptcld, ptcld_name, format='ply')
if sfm.SHOW_POINT_CLOUD:
self.DEBUG_show_point_cloud()
# Finish up
Util.finish_progress_bar()
print ''
print 'Saved files:'
print '\t'+grey_fig_name
print '\t'+color_fig_name
print '\t'+ptcld_name
print ''
