def __getitem__(self, index):
pcd = self.data.iloc[index, 0]
pcd_origin = self.data.iloc[index, 1]
point = pcl.PointCloud_PointXYZRGBA()
point_origin = pcl.PointCloud_PointXYZRGBA()
point.from_file(pcd)
point_origin.from_file(pcd_origin)
point_np = np.zeros((point.size, 4), np.float32)
point_origin_np = np.zeros((point.size, 6), np.float32)
for i in range(point.size):
point_np[i][0] = point[i][0]
point_np[i][1] = point[i][1]
point_np[i][2] = point[i][2]
point_np[i][3] = int(point[i][3]) >> 16 & 0x000ff
if point_np.shape[0] < self.num_point:
row_idx = np.random.choice(point_np.shape[0],
self.num_point,
replace=True)
else:
row_idx = np.random.choice(point_np.shape[0],
self.num_point,
replace=False)
# row_idx = np.sort(row_idx)
point_out = torch.from_numpy(point_np[row_idx, :3]) ## need to revise
label = torch.from_numpy(point_np[row_idx, 3]) ## need to revise
origin_list = []
for i in range(point.size):
point_origin_np[i][0] = point_origin[i][0]
point_origin_np[i][1] = point_origin[i][1]
point_origin_np[i][2] = point_origin[i][2]
point_origin_np[i][3] = int(point_origin[i][3]) >> 0 & 0x000ff
point_origin_np[i][4] = int(point_origin[i][3]) >> 8 & 0x000ff
point_origin_np[i][5] = int(point_origin[i][3]) >> 16 & 0x000ff
#print point_origin_np[0] , point_origin_np[1]
point_origin_np = point_origin_np[row_idx]
# print origin_list[0] , origin_list[1]
point_out = np.transpose(point_out, (1, 0))
target = torch.zeros((self.num_point, 2))
#target = torch.zeros((self.num_point), dtype = torch.long)
for i in range(self.num_point):
#print label[i]
#target[i] = int(label[i])
target[i][int(label[i])] = 1
out = {
'x': point_out,
'y': target,
'origin': point_origin_np,
'path': pcd_origin
}
return out
def prediction(dataiter, network):
batch = dataiter.next()
inputs = Variable(batch['x'].cuda())
origin = Variable(batch['origin'])[0]
labels = Variable(batch['y'].cuda())[0]
output = network(inputs)[0][0]
point = pcl.PointCloud_PointXYZRGBA()
point_origin = pcl.PointCloud_PointXYZRGBA()
print batch['path']
_point_list = []
_origin_list = []
in_point = np.transpose(inputs[0].cpu().numpy(), (1, 0))
print labels, output
for i in range(len(in_point)):
if labels[i].argmax() == 1:
print output[i], labels[i]
red = float(origin[i][3] >> 16)
green = float(origin[i][4] >> 8)
blue = float(origin[i][5] >> 0)
rgb = float(
int(origin[i][5] << 16) | int(origin[i][4] << 8)
| int(origin[i][3]))
#if output[i].argmax() == labels[i] and output[i].argmax() == 1:
if output[i].argmax() == labels[i].argmax() and output[i].argmax(
) == 1:
# if output[i][0] < -0.1:
# # red = np.right_shift(red, 8).astype(np.uint8)
# # green = np.right_shift(green, 8).astype(np.uint8)
# # blue = np.right_shift(blue, 8).astype(np.uint8)
# red = np.asarray(red).astype(np.uint32)
# green = np.asarray(green).astype(np.uint32)
# blue = np.asarray(blue).astype(np.uint32)
# rgb = np.left_shift(red, 16) + np.left_shift(green, 8) + np.left_shift(blue, 0)
_point_list.append(
[inputs[0][0][i], inputs[0][1][i], inputs[0][2][i], rgb])
else:
_origin_list.append(
[inputs[0][0][i], inputs[0][1][i], inputs[0][2][i], rgb])
# rgb = np.left_shift(red, 16) + np.left_shift(green, 8) + np.left_shift(blue, 0)
# ptcloud = np.vstack((in_point[i][0], in_point[i][1], in_point[i][2], rgb)).transpose()
point.from_array(np.asarray(_point_list, dtype=np.float32))
point.to_file("./out.pcd")
point_origin.from_array(np.asarray(_origin_list, dtype=np.float32))
point_origin.to_file("./origin.pcd")
def prediction(dataiter, network):
batch = dataiter.next()
inputs = Variable(batch['x'].cuda())
origin = Variable(batch['origin'])[0]
labels = Variable(batch['y'].cuda())[0]
output = network(inputs)[0][0]
point_origin = pcl.PointCloud_PointXYZRGBA()
_origin_list = []
in_point = np.transpose(inputs[0].cpu().numpy(), (1, 0))
print batch['id']
print output.argmax()
print output
if output.argmax() == labels:
print("right")
else:
print("wrong")
for i in range(len(in_point)):
red = float(origin[i][3] >> 16)
green = float(origin[i][4] >> 8)
blue = float(origin[i][5] >> 0)
rgb = float(
int(origin[i][5] << 16) | int(origin[i][4] << 8)
| int(origin[i][3]))
_origin_list.append(
[inputs[0][0][i], inputs[0][1][i], inputs[0][2][i], rgb])
point_origin.from_array(np.asarray(_origin_list, dtype=np.float32))
point_origin.to_file("./origin.pcd")
def frame_to_world(opt):
"""
project pixel frame to world using matrix operation, for pointCloud generation
input depth, color, intrinsic, camera pose
:param opt: opt has depth color intrinsic camera pose path
:return: pointCloud save in .ply
"""
# input depth, color
depth = misc.imread(os.path.join(opt.depth_path, opt.frame + '.png'))
height, width = depth.shape
color = misc.imread(os.path.join(opt.color_path, opt.frame + '.jpg'))
color = misc.imresize(color, (height, width)).astype(np.int64)
# input intrinsic
intrinsic = []
with open(opt.intrinsic, 'r') as f:
line = f.readline()
while line:
intrinsic.append(line.split())
line = f.readline()
intrinsic = np.array(intrinsic, dtype=np.float32)
intrinsic = adjust_intrinsic(intrinsic, [640, 480], [height, width])
intrinsic_inv = np.linalg.inv(intrinsic)
# camera pose = extrinsic_inv
pose = []
with open(os.path.join(opt.pose_path, opt.frame + '.txt'), 'r') as f:
line = f.readline()
while line:
pose.append(line.split())
line = f.readline()
extrinsic_inv = np.array(pose, dtype=np.float32)
# extrinsic = np.linalg.inv(extrinsic_inv)
# reshape depth color to [height * width,]
depth = depth.reshape(-1)
color = color.reshape(-1, 3)
# row -> i col -> j
row = torch.arange(height).view(height, 1).repeat(
1, width).view(-1).float().cpu().numpy()
col = torch.arange(width).view(1, width).repeat(
1, height).view(-1).float().cpu().numpy()
# pixel frame [depth * j, depth * i, depth, 1]
pixel_frame = np.zeros((4, depth.shape[0]))
pixel_frame[0, :][depth != 0] = depth[depth != 0] * col[depth != 0]
pixel_frame[1, :][depth != 0] = depth[depth != 0] * row[depth != 0]
pixel_frame[2, :][depth != 0] = depth[depth != 0]
pixel_frame[3, :][depth != 0] = 1
# camera coordinate [Xc, Yc, Zc, 1]
camera_coor = np.dot(intrinsic_inv, pixel_frame)
# world coordinate [X, Y, Z, 1]
world_coor = np.dot(extrinsic_inv, camera_coor)
# RGBA for pointCloud
RGBA = np.zeros((1, height * width)).astype(np.float64)
RGBA[0, :][depth != 0] = (0xff & color[:, 0][depth != 0]) << 16 | (
0xff & color[:, 1][depth != 0]) << 8 | (0xff & color[:, 0][depth != 0])
# generate points [X, Y, Z, RGBA]
points = np.concatenate((world_coor[0:3], RGBA), axis=0).T
points = np.array(points, dtype=np.float32)
# PCL Cloud
cloud = pcl.PointCloud_PointXYZRGBA()
cloud.from_array(points)
pcl.save(cloud, opt.frame + '.ply', format='ply')
def main():
# RGB : NG
# f = file.File('28XXX10000075-18.las', mode='r')
f = file.File('28W0608011101-1.las', mode='r')
# f = file.File('28XXX00020001-1.las', mode='r')
# f = file.File('simple1_4.las', mode='r')
# check las file version
# RGB contains
if f._header.data_format_id in (2, 3, 5):
red = (f.red)
green = (f.green)
blue = (f.blue)
# 16bit to convert 8bit data(data Storage First 8 bits case)
red = np.right_shift(red, 8).astype(np.uint8)
green = np.right_shift(green, 8).astype(np.uint8)
blue = np.right_shift(blue, 8).astype(np.uint8)
# (data Storage After 8 bits case)
# red = red.astype(np.uint8)
# green = green.astype(np.uint8)
# blue = blue.astype(np.uint8)
red = red.astype(np.uint32)
green = green.astype(np.uint32)
blue = blue.astype(np.uint32)
rgb = np.left_shift(red, 16) + np.left_shift(green, 8) + np.left_shift(
blue, 0)
ptcloud = np.vstack((f.x, f.y, f.z, rgb)).transpose()
cloud = pcl.PointCloud_PointXYZRGBA()
# set raw points
# cloud.from_array(np.array(ptcloud, dtype=np.float32))
# set point centered
mean_param = np.concatenate([np.mean(ptcloud, 0)[0:3], np.zeros(1)])
ptcloud_centred = ptcloud - mean_param
# print(ptcloud_centred)
cloud.from_array(np.array(ptcloud_centred, dtype=np.float32))
# Visualization
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorACloud(cloud, b'cloud')
else:
ptcloud = np.vstack((f.x, f.y, f.z)).transpose()
mean_param = np.mean(ptcloud, 0)
cloud = pcl.PointCloud()
# set raw points
# cloud.from_array(np.array(ptcloud, dtype=np.float32))
# set point centered
# mean_param = np.concatenate([np.mean(ptcloud, 0)[0:3], np.zeros(1)])
ptcloud_centred = ptcloud - mean_param
# print(ptcloud_centred)
cloud.from_array(np.array(ptcloud_centred, dtype=np.float32))
# Visualization
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowMonochromeCloud(cloud, b'cloud')
v = True
while v:
v = not (visual.WasStopped())
def setUp(self):
rng = np.random.RandomState(42)
# Define two dense sets of points of sizes 30 and 170, resp.
a = rng.randn(100, 4).astype(np.float32)
a[:30] -= 42
self.pc = pcl.PointCloud_PointXYZRGBA(a)
self.kd = pcl.KdTreeFLANN_PointXYZRGBA(self.pc)
def view_cloud(pointcloud):
cloud = pcl.PointCloud_PointXYZRGBA()
cloud.from_array(pointcloud)
try:
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorACloud(cloud)
v = True
while v:
v = not (visual.WasStopped())
except:
pass
def setUp(self):
self.p = pcl.load("tutorials/table_scene_lms400.pcd")
self.fil = self.p.make_ApproximateVoxelGrid()
self.p2 = pcl.PointCloud_PointXYZI()
self.fil2 = self.p2.make_ApproximateVoxelGrid()
self.p3 = pcl.PointCloud_PointXYZRGB()
self.fil3 = self.p3.make_ApproximateVoxelGrid()
self.p4 = pcl.PointCloud_PointXYZRGBA()
self.fil4 = self.p4.make_ApproximateVoxelGrid()
def createPointCloud(self, images, poses, cameraMatrix):
## Fill cloud structure
# pcl_points = pcl.PointCloud()
print("\nCreating resulting point cloud...")
print("This may take a few minutes...")
## Per camera
pcl_p = pcl.PointCloud_PointXYZRGBA()
all_points = np.zeros(shape=(1, 4), dtype=np.float32)
bad_depths = []
for c in range(len(poses)):
image = images[c]
R = poses[c].R
t = poses[c].t
if R.size == 0:
continue
for i in range(image.dep.shape[0]):
rgbs = image.rgb[i]
deps = image.dep[i]
for j in range(image.dep.shape[1]):
rgb = rgbs[j]
dep = deps[j]
if dep < self.low_thresh or dep > self.high_thresh:
bad_depths.append(dep)
continue
point = util.backproject3D(j, i, dep, cameraMatrix)
gPoint = np.transpose(R) @ point - t
point = gPoint
rgb_temp = rgb[2] << 16 | rgb[1] << 8 | rgb[0]
point_ext = np.array(
[[point[0][0], point[1][0], point[2][0], rgb_temp]],
dtype=np.float32)
single_point = point_ext
all_points = np.concatenate((all_points, single_point),
axis=0)
pcl_p.from_array(all_points)
# pcl_p_reduced = self.reduceCloud(pcl_p)
print("Generated {} points.".format(len(pcl_p.to_list())))
return pcl_p
def DisplayNumpyColorCloudPCL(cloud):
visual = pcl.pcl_visualization.CloudViewing()
if type(cloud).__module__ == np.__name__:
# ptcloud_centred = pcl.PointCloud()
# ptcloud_centred = pcl.PointCloud_PointXYZI()
ptcloud_centred = pcl.PointCloud_PointXYZRGBA()
ptcloud_centred.from_array(np.array(cloud, dtype=np.float32))
# visual.ShowGrayCloud(ptcloud_centred, b'cloud')
visual.ShowColorACloud(ptcloud_centred, b'cloud')
# visual.ShowMonochromeCloud(ptcloud_centred, b'cloud')
else:
visual.ShowMonochromeCloud(cloud, b'cloud')
v = True
while v:
v = not (visual.WasStopped())
def viewPointClouds(folderDirectory, full):
"""
Function which allows for the viewing of pointClouds
Parameters
----------
folderDirectory : str
Directory containing .pickle files of saved frames from capture
full : int
0 if you wish to view every 10 frames, 1 if you wish to view every frame
"""
p = multiprocessing.Pool(
6
) #create a multiprocessing pool to efficiently create pointclouds in parallel
visual = pcl.pcl_visualization.CloudViewing(
) #handle for PCL point cloud viewer
signal.signal(signal.SIGINT, signalHandler)
i = 0
while True:
if not full:
i += 10
print(i)
fname = folderDirectory + '\\' + str(format(i, '05d')) + '.pickle'
file = open(fname, 'rb')
frame = pickle.load(file)
file.close()
#i+=1
cloud = pcl.PointCloud_PointXYZRGBA()
listP = [data for serial, data in frame.items()]
try:
cameraPoints = p.map(
depthFrametoPC, listP
) #process point cloud for each camera in a separate thread
allPoints = np.empty((0, 4))
for camera in cameraPoints:
allPoints = np.append(allPoints, camera, axis=0)
cloud.from_array(allPoints.astype('float32'))
visual.ShowColorACloud(cloud)
i += 1
except KeyboardInterrupt:
p.terminate()
def setUp(self):
self.a = np.array(np.mat(SEGDATA, dtype=np.float32))
self.p = pcl.PointCloud_PointXYZRGBA()
self.p.from_array(self.a)
def test_asarray(self):
p = pcl.PointCloud_PointXYZRGBA(self.p) # copy
# old0 = p[0]
a = np.asarray(p) # view
a[:] += 6
assert_array_almost_equal(p[0], a[0])
# 求解相机坐标到世界坐标的转换矩阵
pose = poses[i]
R, t = get_transform(pose)
# 求解相机坐标系坐标
row_mat = np.array([list(range(size[0]))] * size[1]).transpose()
column_mat = np.array([list(range(size[1]))] * size[0])
z_mat = depth_img / depthScale
x_mat = np.multiply((column_mat - cx) / fx, z_mat)
y_mat = np.multiply((row_mat - cy) / fy, z_mat)
# 去除零点
Pc_mat = np.stack((x_mat, y_mat, z_mat), axis=-1).reshape((-1, 3))
rgb_mat = np.expand_dims(
(color_img[:, :, 2] * 16**4 + color_img[:, :, 1] * 16**2 +
color_img[:, :, 0]).flatten(),
axis=-1)
zero_columns = np.where(Pc_mat[:, 2] == 0)[0]
Pc_mat = np.delete(Pc_mat, zero_columns, 0)
rgb_mat = np.delete(rgb_mat, zero_columns, 0)
# 求解世界坐标系坐标
Pw_mat = np.dot(R, Pc_mat.transpose()) + np.expand_dims(t, axis=-1)
points.append(np.concatenate((Pw_mat.transpose(), rgb_mat), axis=-1))
points = np.concatenate(points)
p = pcl.PointCloud_PointXYZRGBA([[point[0], point[1], point[2],
int(point[3])] for point in points])
pcl.save_XYZRGBA(p, 'map.pcd')
def setUp(self):
self.p = pcl.PointCloud_PointXYZRGBA(
np.arange(12, dtype=np.float32).reshape(3, 4))
def setUp(self):
self.p = pcl.PointCloud_PointXYZRGBA(_data)
def test_copy():
a = np.random.randn(100, 4).astype(np.float32)
p1 = pcl.PointCloud_PointXYZRGBA(a)
p2 = pcl.PointCloud_PointXYZRGBA(p1)
assert_array_equal(p2.to_array(), a)
def main():
# | fx 0 cx |
# | 0 fy cy |
# | 0 0 1 |
# vals = np.array(
# [525., 0. , 319.5,
# 0. , 525., 239.5,
# 0. , 0. , 1.])
# cameraMatrix = vals.reshape((3, 3))
# grabber_sequences/pclzf/*.xml
cameraMatrix = np.array([[525., 0., 320.0], [0., 525., 240.0],
[0., 0., 1.]])
# color0 = cv2.imread('rgb/0.png')
color0 = cv2.imread(
'grabber_sequences/tiff/frame_20121214T142255.814212_rgb.tiff')
# 16 bit Image
# https://github.com/eiichiromomma/CVMLAB/wiki/OpenCV-16bitImage
# depth0 = cv2.imread('depth/0.png', -1)
# depth0 = cv2.imread('depth/0.png', cv2.IMREAD_ANYDEPTH | cv2.IMREAD_ANYCOLOR)
depth0 = cv2.imread(
'grabber_sequences/tiff/frame_20121214T142255.814212_depth.tiff',
cv2.IMREAD_ANYDEPTH | cv2.IMREAD_ANYCOLOR)
print("Color: ", color0.dtype)
print("Depth: ", depth0.dtype)
# colorImage1 = cv2.imread('rgb/1.png')
# depth1 = cv2.imread('depth/1.png', -1)
# if (color0.empty() || depth0.empty() || colorImage1.empty() || depth1.empty()):
# cout << "Data (rgb or depth images) is empty.";
# return -1;
# gray0 = cv2.cvtColor(color0, cv2.COLOR_BGR2GRAY)
# grayImage1 = cv2.cvtColor(colorImage1, cv2.COLOR_BGR2GRAY)
# depthFlt0 = depth0.convertTo(cv2.CV_32FC1, 1. / 1000.0)
# depthFlt1 = depth1.convertTo(cv2.CV_32FC1, 1. / 1000.0)
depthFlt0 = np.float32(depth0) / 1000.0
# depthFlt0 = depth0 / 1000.0
# depthFlt1 = np.float32(depth1) / 1000.0
import pcl
# points0 = cvtDepth2Cloud(depthFlt0, cameraMatrix)
# cloud0 = pcl.PointCloud()
points0 = cvtDepthColor2Cloud(depthFlt0, color0, cameraMatrix)
cloud0 = pcl.PointCloud_PointXYZRGBA()
cloud0.from_array(points0)
print(cloud0)
# points1 = cvtDepth2Cloud(depthFlt1, cameraMatrix)
# cloud1 = pcl.PointCloud()
# cloud1.from_array(points1)
# wait
try:
import pcl.pcl_visualization
visual = pcl.pcl_visualization.CloudViewing()
# xyz only
# visual.ShowMonochromeCloud(cloud0)
# visual.ShowMonochromeCloud(cloud1)
# color(rgba)
visual.ShowColorACloud(cloud0)
v = True
while v:
v = not (visual.WasStopped())
except:
pass
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')