def off2pc_vertices(path):
i = 0
with open(path, "r") as file:
x = file.read().splitlines()
print(x)
while i < len(x):
filename = x[i]
#load OFF-file
mesh = trimesh.load('OFF_Files/m' + filename + '.off')
points = np.asarray(mesh.vertices)
point_cloud = op.PointCloud()
point_cloud.points = op.Vector3dVector(points)
# has to be changed:
# initial folder where the data is located ("benchmark_files_vertices")
# class ("vehicle")
# "train" or "test"
# name of the file ("vehicle_")
op.io.write_point_cloud(
'classification/v1/benchmark_files_vertices/vehicle/train/vehicle_'
+ filename + '.pcd',
point_cloud,
write_ascii=True,
compressed=False)
i = i + 1
return x
def plot_3d_pc(pc, uniform_color=False):
plot_pc = o3d.PointCloud()
plot_pc.points = o3d.Vector3dVector(pc)
if uniform_color == True:
plot_pc.paint_uniform_color([0, 0.651, 0.929]) # yellow
# [1, 0.706, 0] blue
o3d.visualization.draw_geometries([plot_pc])
def off2pc_merged(path):
i = 0
# get the files
with open(path, "r") as file:
x = file.read().splitlines()
print(x)
# run through all files
while i < len(x):
filename = x[i]
#load OFF-file
mesh = trimesh.load('OFF_Files/m' + filename + '.off')
# generate points on surfaces and points on edges
points = np.asarray(mesh.vertices)
points_sampled = np.asarray(sample_pc(mesh, 1000))
# merge points of two point clouds
points_merged = np.vstack((points_sampled, points))
point_cloud = op.PointCloud()
point_cloud.points = op.Vector3dVector(points_merged)
#save the resulting point cloud
op.io.write_point_cloud(
'classification/v1/benchmark_files_test/animal/test/animal_' +
filename + '.pcd',
point_cloud,
write_ascii=True,
compressed=False)
i = i + 1
return x
def upscale(pc, pcd, pcd_tree):
points = np.asarray(pc.points)
colors = np.asarray(pc.colors)
normals = np.asarray(pc.normals)
dnormals = np.asarray(pcd.normals)
n = -1
for (point, color, normal) in zip(points[:n], colors[:n], normals[:n]):
[k, idx, _] = pcd_tree.search_knn_vector_3d(point, 10)
knn = points[idx, :]
knn_rel = knn - point
knn_distance = np.linalg.norm(knn_rel, axis=1)
max_ = np.max(knn_distance)
reverse_knn = knn_distance * -1 + max_
normalized_knn = np.expand_dims(reverse_knn / np.sum(reverse_knn),
axis=-1)
ncols = dnormals[idx, :] * normalized_knn
ncols = ncols.sum(axis=0)
normal[:] = ncols
pc.normals = open3d.Vector3dVector(normals)
return pc
def prepare_source_and_target_rigid_3d(source_filename,
noise_amp=0.001,
n_random=500,
orientation=np.deg2rad([0.0, 0.0,
30.0]),
translation=np.zeros(3),
normals=False):
source = o3.read_point_cloud(source_filename)
source = o3.voxel_down_sample(source, voxel_size=0.005)
print(source)
target = copy.deepcopy(source)
tp = np.asarray(target.points)
np.random.shuffle(tp)
rg = 1.5 * (tp.max(axis=0) - tp.min(axis=0))
rands = (np.random.rand(n_random, 3) - 0.5) * rg + tp.mean(axis=0)
target.points = o3.Vector3dVector(
np.r_[tp + noise_amp * np.random.randn(*tp.shape), rands])
ans = trans.euler_matrix(*orientation)
ans[:3, 3] = translation
target.transform(ans)
if normals:
estimate_normals(
source, o3.geometry.KDTreeSearchParamHybrid(radius=0.3, max_nn=50))
estimate_normals(
target, o3.geometry.KDTreeSearchParamHybrid(radius=0.3, max_nn=50))
return source, target
def open3d_icp(src, trgt, init_rotation=np.eye(3, 3)):
source = open3d.PointCloud()
source.points = open3d.Vector3dVector(src)
target = open3d.PointCloud()
target.points = open3d.Vector3dVector(trgt)
init_rotation_4x4 = np.eye(4, 4)
init_rotation_4x4[0:3, 0:3] = init_rotation
threshold = 0.2
reg_p2p = open3d.registration_icp(
source, target, threshold, init_rotation_4x4,
open3d.TransformationEstimationPointToPoint())
return reg_p2p
def get_all_smpl(pkl_data, json_data):
gender = json_data['people'][0]['gender_gt']
all_meshes = []
trans = np.array([4, 0, 0])
for i, result in enumerate(pkl_data['all_results']):
t = trans + [0, i * 3, 0]
betas = torch.Tensor(result['betas']).unsqueeze(0)
pose = torch.Tensor(result['body_pose']).unsqueeze(0)
transl = torch.Tensor(result['transl']).unsqueeze(0)
global_orient = torch.Tensor(result['global_orient']).unsqueeze(0)
model = smplx.create('models', model_type='smpl', gender=gender)
output = model(betas=betas, body_pose=pose, transl=transl, global_orient=global_orient, return_verts=True)
smpl_vertices = output.vertices.detach().cpu().numpy().squeeze()
smpl_o3d = o3d.TriangleMesh()
smpl_o3d.triangles = o3d.Vector3iVector(model.faces)
smpl_o3d.vertices = o3d.Vector3dVector(smpl_vertices)
smpl_o3d.compute_vertex_normals()
smpl_o3d.translate(t)
for idx, key in enumerate(result['loss_dict'].keys()):
lbl = '{} {:.2f}'.format(key, float(result['loss_dict'][key]))
all_meshes.append(text_3d(lbl, t + [1, idx * 0.2 - 1, 2], direction=(0.01, 0, -1), degree=-90, font_size=150, density=0.2))
all_meshes.append(smpl_o3d)
return all_meshes
def cb_save_ply(msg):
d = outPn.astype(np.float32)
pc = o3d.PointCloud()
pc.points = o3d.Vector3dVector(d)
print 'save model PC', d.dtype, d.shape
o3d.write_point_cloud(Args["wd"] + "/sample.ply", pc, True, False)
return
def main():
script_dir = os.getcwd()
config = pyreg.functions.read_config()
bunny_raw = PointCloud()
try:
bunny_raw.read_pcd(config["files"]["source"])
except (SystemExit, KeyboardInterrupt):
raise
except Exception:
raise Exception("File not found!")
bunny_raw.center_around_origin()
bunny_raw_open3d = open3d.PointCloud()
bunny_raw_open3d.points = open3d.Vector3dVector(bunny_raw.points)
bunny_open3d = open3d.voxel_down_sample(
bunny_raw_open3d, voxel_size=config["downsample"]["voxel_size"])
bunny = PointCloud(
points=copy.deepcopy(numpy.asarray(bunny_open3d.points)))
mlab.figure()
bunny_pts = mlab.points3d(bunny.points[:, 0],
bunny.points[:, 1],
bunny.points[:, 2],
color=(0, 1, 0),
mode="point")
mlab.outline()
ipdb.set_trace()
bunny.write_pcd(file_format="txt",
file_path=config["files"]["output"] + "bunny_7000.xyz")
def main():
home_dir = expanduser("~")
parser = argparse.ArgumentParser()
parser.add_argument(
"--img_dir",
help="path to the directory that saves" " depth and rgb images from ORB_SLAMA2",
default="%s/.ros/Imgs" % home_dir,
type=str,
)
parser.add_argument(
"--depth_max", default=1.5, help="maximum value for the depth", type=float
)
parser.add_argument(
"--depth_min", default=0.2, help="minimum value for the depth", type=float
)
parser.add_argument(
"--cfg_filename",
default="realsense_d435.yaml",
help="which config file to use",
type=str,
)
parser.add_argument(
"--subsample_pixs",
default=1,
help="sample every n pixels in row/column"
"when the point cloud is reconstructed",
type=int,
)
args = parser.parse_args()
rospy.init_node("reconstruct_world", anonymous=True)
rgb_dir = os.path.join(args.img_dir, "RGBImgs")
depth_dir = os.path.join(args.img_dir, "DepthImgs")
pcd_processor = PointCloudProcessor(
rgb_dir=rgb_dir,
depth_dir=depth_dir,
cfg_filename=args.cfg_filename,
subsample_pixs=args.subsample_pixs,
depth_threshold=(args.depth_min, args.depth_max),
)
time.sleep(2)
points, colors = pcd_processor.get_current_pcd()
if USE_OPEN3D:
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(points)
pcd.colors = open3d.Vector3dVector(colors / 255.0)
coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
open3d.draw_geometries([pcd, coord])
def o3d_trace_rays(cam_pose,
max_range=5.5,
fov_h_angle=0.34 * np.pi,
fov_v_angle=0.25 * np.pi,
angle_gap=0.01 * np.pi,
color=[0.5, 1, 0.5]):
ray_set = open3d.LineSet()
cam_forward = np.array([0, 0, 1])
cam_left = np.array([-1, 0, 0])
cam_up = np.array([0, -1, 0])
points = []
lines = []
points.append(cam_pose[:3, 3].tolist())
# prepare the list of view angles
fov_h_angle_list = np.arange(start=fov_h_angle / 2,
stop=-fov_h_angle / 2,
step=-angle_gap)
fov_h_angle_list = np.append(fov_h_angle_list, -fov_h_angle /
2) # ensure the last value is included
fov_v_angle_list = np.arange(start=fov_v_angle / 2,
stop=-fov_v_angle / 2,
step=-angle_gap)
fov_v_angle_list = np.append(fov_v_angle_list, -fov_v_angle /
2) # ensure the last value is included
for angle_v in fov_v_angle_list:
up_offset = cam_up * np.tan(angle_v)
for angle_h in fov_h_angle_list:
left_offset = cam_left * np.tan(angle_h)
direction_unit = cam_forward + up_offset + left_offset
direction_unit = direction_unit / np.linalg.norm(direction_unit)
direction_unit = np.dot(cam_pose[:3, :3], direction_unit)
point = cam_pose[:3, 3] + direction_unit * max_range
points.append(point.tolist())
for i in range(len(points) - 1):
line = [0, i + 1]
lines.append(line)
colors = [color for i in range(len(lines))]
# get vector camera_look_at
# set ray_set
ray_set.points = open3d.Vector3dVector(points)
ray_set.lines = open3d.Vector2iVector(lines)
ray_set.colors = open3d.Vector3dVector(colors)
return ray_set
def main():
# Load calibration data
calibration_data = calibration.CalibrationData.load("calibration.json")
# Calc rectification maps
rectification_maps, disp_to_depth = stereo.get_undistorted_rectification_maps(calibration_data)
# Load images
left_image = cv.imread("captures/1533479723660-left.png")
right_image = cv.imread("captures/1533479723660-right.png")
# Convert to grayscale images
left_gray = cv.cvtColor(left_image, cv.COLOR_BGR2GRAY)
right_gray = cv.cvtColor(right_image, cv.COLOR_BGR2GRAY)
# Rectify images
left_rectified, right_rectified = stereo.rectify(left_gray, right_gray, rectification_maps)
# Calculate disparity map
stereoBm = cv.StereoBM_create(numDisparities=272, blockSize=5)
disparity = stereoBm.compute(left_rectified, right_rectified)
# Convert disparity map to float32 map. This is important for the 3d reprojection to work
disparity = disparity.astype(np.float32) * 1.0/255
# Calculate distances
distances = cv.reprojectImageTo3D(disparity, disp_to_depth, True)
# Calculate points. distances is of shape w*h*3 for a w*h image.
# Convert it to a n*3 array (n=w*h), e.g.: an array of points
points = np.reshape(distances, (-1, 3))
# Get colors for each point from the left (unrectified) image of the camera.
colors = np.reshape(left_image, (-1, 3)).astype(np.float32) / 255.0
# Transform BGR to RGB
colors = colors[..., ::-1]
# Filter points. Remove all points with very low z or z bigger than a certain value.
# The disparity values for these points were not calculated correctly
points_mask = np.logical_and(points[..., 2] > -100000, points[..., 2] < -2600)
colors = colors[points_mask]
points = points[points_mask]
# Render point cloud
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(points * np.array((1, 1, -1)))
pcd.colors = open3d.Vector3dVector(colors)
open3d.draw_geometries([pcd])
def down_sampling_pc(pc):
#print('shape of original pc is', np.shape(pc))
down_pcd = o3d.geometry.PointCloud()
down_pcd.points = o3d.Vector3dVector(pc)
pc_ds = o3d.geometry.voxel_down_sample(down_pcd, 2)
pc_ds_points = pc_ds.points
#print('shape of downsample pc is', np.shape(pc_ds_points))
return pc_ds_points
def load_point_cloud(omsws, part='full'):
# create point cloud having point cloud of scan points
scans = read_scans_pc(omsws, part=part)
# Pc = OMS_PointCloud()
Pc = o3d.PointCloud()
Pc.points = o3d.Vector3dVector(scans)
return Pc
def check_pointcloud_type(self, pointcloud):
if isinstance(pointcloud, np.ndarray):
pcd = open3d.PointCloud()
print type(pointcloud)
print 'what is getting', pointcloud.shape
pcd.points = open3d.Vector3dVector(pointcloud)
pointcloud = pcd
return pointcloud
def np_to_pcd(xyz):
"""
convert numpy array to point cloud object in open3d
"""
xyz = xyz.reshape(-1, 3)
pcd = o3d.PointCloud()
pcd.points = o3d.Vector3dVector(xyz)
return pcd
def voxel_downsample(pts, voxel_size):
pc = o3d.PointCloud()
# print(pts.shape)
pc.points = o3d.Vector3dVector(pts.cpu().numpy())
v_pc = o3d.voxel_down_sample(pc, voxel_size=voxel_size)
v_pts = torch.Tensor(np.asarray(v_pc.points)).to(pts.device)
idx = square_distance(v_pts.unsqueeze(0), pts.unsqueeze(0))[0].topk(k=1, dim=1, largest=False)[1].view(-1)
return idx
def draw_pc(pc_xyzrgb):
"""
Plot pointcloud with color
Parameter:
pc_xyzrgb: [[x, y, z, r, g, b],...]
"""
pc = open3d.PointCloud()
pc.points = open3d.Vector3dVector(pc_xyzrgb[:, 0:3])
if pc_xyzrgb.shape[1] == 3:
open3d.draw_geometries([pc])
return 0
if np.max(pc_xyzrgb[:, 3:6]) > 20: ## 0-255
pc.colors = open3d.Vector3dVector(pc_xyzrgb[:, 3:6] / 255.)
else:
pc.colors = open3d.Vector3dVector(pc_xyzrgb[:, 3:6])
open3d.draw_geometries([pc])
return 0
def display_prediction(pc, pred):
points = np.array(pc).reshape(
(np.array(pc).shape[0] * 5, 4000, 3)).reshape(
(np.array(pred).shape[0] * 5 * 4000, 3))
annotations = np.array(pred).reshape(
(np.array(pred).shape[0] * 5, 4000, 2)).reshape(
(np.array(pred).shape[0] * 5 * 4000, 2))
colors = []
for a in annotations:
if (a[0] > a[1]):
colors.append(np.array([1.0, 0.0, 0.0]))
else:
colors.append(np.array([0.0, 1.0, 0.0]))
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(points)
pcd.colors = open3d.Vector3dVector(np.array(colors))
open3d.draw_geometries([pcd])
def draw_pointclouds(pc=None, shape_name=None):
if pc is None:
print("No input file is given!")
exit
pcd_input = o3d.PointCloud() # input point cloud (XYZ + RGB)
pcd_pred = o3d.PointCloud() # prediction point cloud (XYZ + RGB)
pcd_gt = o3d.PointCloud() # ground truth point cloud (XYZ + RGB)
pcd_input.points = o3d.Vector3dVector(pc[:, 0:3]) # XYZ coordinates
pcd_pred.points = o3d.Vector3dVector(pc[:, 0:3]) # XYZ coordinates
pcd_gt.points = o3d.Vector3dVector(pc[:, 0:3]) # XYZ coordinates
# pcd_input.colors = o3d.Vector3dVector([30,30,30]/ 255.0) # open3d requires colors (RGB) to be in range[0,1]
pcd_pred.colors = o3d.Vector3dVector(label_to_color(pc[:, -2]) / 255.0)
pcd_gt.colors = o3d.Vector3dVector(label_to_color(pc[:, -1]) / 255.0)
# render and crop the input point cloud
# o3d.visualization.draw_geometries_with_editing([pcd_input], window_name=shape_name)
# record the cropping operation and params
times_of_cropping = 0
for item in os.listdir(BASE_DIR):
if item.startswith('cropped') and item.endswith('.json'):
times_of_cropping += 1
print 'times of cropping:', times_of_cropping
# crop the input, pred and gt point cloud
pcd_input_cropped = pcd_input
pcd_pred_cropped = pcd_pred
pcd_gt_cropped = pcd_gt
for crop_seq in range(times_of_cropping):
spv = o3d.visualization.read_selection_polygon_volume(
"cropped_%d.json" % (crop_seq + 1))
pcd_input_cropped = spv.crop_point_cloud(pcd_input_cropped)
pcd_pred_cropped = spv.crop_point_cloud(pcd_pred_cropped)
pcd_gt_cropped = spv.crop_point_cloud(pcd_gt_cropped)
# custom_draw_geometry_with_key_callback(pcd_input_cropped, os.path.join(SHAPE_DIR, shape_name+'_1input.png'),
# window_name = shape_name + ' input point cloud')
custom_draw_geometry_with_key_callback(
pcd_pred_cropped,
os.path.join(SHAPE_DIR, shape_name + '_2pred.png'),
window_name=shape_name + ' prediction result')
custom_draw_geometry_with_key_callback(
pcd_gt_cropped,
os.path.join(SHAPE_DIR, shape_name + '_3gt.png'),
window_name=shape_name + ' ground truth')
def downsample(pcd, voxel_size):
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(pcd.points)
pcd_open3d = open3d.voxel_down_sample(pcd_open3d, voxel_size=voxel_size)
pcd = PointCloud(points=copy.deepcopy(numpy.asarray(pcd_open3d.points)))
return pcd
def post_process(affordance_map, class_pred,
color_input, color_bg,
depth_input, depth_bg, camera_intrinsic):
## scale the images to the proper value
color_input = np.asarray(color_input, dtype=np.float32) / 255
color_bg = np.asarray(color_bg, dtype=np.float32) / 255
depth_input = np.asarray(depth_input, dtype=np.float64) / 10000
depth_bg = np.asarray(depth_bg, dtype=np.float64) / 10000
## get foreground mask
frg_mask_color = ~(np.sum(abs(color_input-color_bg) < 0.3, axis=2) == 3)
frg_mask_depth = np.logical_and((abs(depth_input-depth_bg) > 0.02), (depth_bg != 0))
foreground_mask = np.logical_or(frg_mask_color, frg_mask_depth)
## project depth to camera space
pix_x, pix_y = np.meshgrid(np.arange(640), np.arange(480))
cam_x = (pix_x - camera_intrinsic[0][2]) * depth_input/camera_intrinsic[0][0]
cam_y = (pix_y - camera_intrinsic[1][2]) * depth_input/camera_intrinsic[1][1]
cam_z = depth_input
depth_valid = (np.logical_and(foreground_mask, cam_z) != 0)
input_points = np.array([cam_x[depth_valid], cam_y[depth_valid], cam_z[depth_valid]]).transpose()
## get the foreground point cloud
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(input_points)
open3d.geometry.estimate_normals(pcd,
search_param=open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 50)
)
## flip normals to point towards camera
open3d.geometry.orient_normals_towards_camera_location(pcd, np.array([0.,0.,0.]))
pcd_normals = np.asarray(pcd.normals)
## reproject the normals back to image plane
pix_x = np.round((input_points[:,0] * camera_intrinsic[0][0] / input_points[:,2] + camera_intrinsic[0][2]))
pix_y = np.round((input_points[:,1] * camera_intrinsic[1][1] / input_points[:,2] + camera_intrinsic[1][2]))
surface_normals_map = np.zeros(color_input.shape)
n = 0
for n, (x,y) in enumerate(zip(pix_x, pix_y)):
x,y = int(x), int(y)
surface_normals_map[y,x,0] = pcd_normals[n,0]
surface_normals_map[y,x,1] = pcd_normals[n,1]
surface_normals_map[y,x,2] = pcd_normals[n,2]
## Compute standard deviation of local normals (baseline)
mean_std_norms = np.mean(stdev_filter(surface_normals_map, 25), axis=2)
baseline_score = 1 - mean_std_norms/mean_std_norms.max()
## Set affordance to 0 for regions with high surface normal variance
affordance_map[baseline_score < 0.1] = 0
affordance_map[~foreground_mask] = 0
class_pred[baseline_score < 0.1] = 0
class_pred[~foreground_mask] = 0
affordance_map[~class_pred.astype(np.bool)] = 0
affordance_map = gaussian_filter(affordance_map, 4)
return surface_normals_map, affordance_map, class_pred
def mesh2pcl(mesh, step):
vertices = np.asarray(mesh.vertices)
vertices = pd.DataFrame(vertices).apply(lambda r: tuple(r),
axis=1).apply(np.array)
vertices = pd.DataFrame(vertices, columns=['vertex'])
triangles = pd.DataFrame(np.asarray(mesh.triangles),
columns=['v1', 'v2', 'v3'])
triangles['v1'] = triangles.merge(vertices,
how='left',
left_on='v1',
right_index=True)['vertex']
triangles['v2'] = triangles.merge(vertices,
how='left',
left_on='v2',
right_index=True)['vertex']
triangles['v3'] = triangles.merge(vertices,
how='left',
left_on='v3',
right_index=True)['vertex']
#generating orth coordinate system
triangles['AB'] = triangles['v2'] - triangles['v1']
triangles['|AB|'] = norm_pandas(triangles['AB'])
triangles['too_sparse'] = triangles['|AB|'] > step
triangles['AC'] = triangles['v3'] - triangles['v1']
triangles['i'] = triangles['AB']
triangles['i'] = triangles['i'] / triangles['|AB|']
triangles['j'] = triangles['AC']
triangles['j'] = triangles['j'] - scalar_product_pandas(
triangles['j'], triangles['i']) * triangles['i']
triangles['j'] = triangles['j'] / norm_pandas(triangles['j'])
triangles['AC.j'] = scalar_product_pandas(triangles['j'], triangles['AC'])
triangles['too_sparse'] = triangles['too_sparse'] | (triangles['AC.j'] >
step)
points = []
triangles = triangles[triangles['too_sparse']]
bar = tqdm.tqdm(total=len(triangles))
for i, triangle in triangles.iterrows():
y = np.linspace(0, 1, max(0, int(triangle['AC.j'] // step)))
x = np.linspace(0, 1, max(0, int(triangle['|AB|'] // step)))
X, Y = np.meshgrid(x, y)
XY = np.array([X.flatten(), Y.flatten()]).T
XY = XY[(XY[:, 1] + XY[:, 0] < 1) & (XY[:, 1] + XY[:, 0] > 0)]
tmp = np.ones((XY.shape[0],1)).dot(np.array([triangle['v1']])) +\
np.array([XY[:,0]]).T.dot(np.array([triangle['AB']])) +\
np.array([XY[:,1]]).T.dot(np.array([triangle['AC']]))
points.append(tmp)
bar.update(1)
bar.close()
points = np.concatenate(points)
points = np.concatenate((points, np.asarray(mesh.vertices)), axis=0)
pcl = pn.PointCloud()
pcl.points = pn.Vector3dVector(points)
return pcl
def save_local_maps(seq):
print(seq)
sequence = dataset.sequence(seq)
seqdir = os.path.join('kitti', '{:03d}'.format(seq))
util.makedirs(seqdir)
istart, imid, iend = get_map_indices(sequence)
maps = []
with progressbar.ProgressBar(max_value=len(iend)) as bar:
for i in range(len(iend)):
scans = []
for idx, val in enumerate(range(istart[i], iend[i])):
velo = sequence.get_velo(val)
scan = o3.PointCloud()
scan.points = o3.Vector3dVector(velo[:, :3])
scan.colors = o3.Vector3dVector(
util.intensity2color(velo[:, 3]))
scans.append(scan)
T_m_w = T_m_mc.dot(T_mc_cam0).dot(
util.invert_ht(sequence.poses[imid[i]]))
T_w_velo = np.matmul(sequence.poses[istart[i]:iend[i]],
sequence.calib.T_cam0_velo)
T_m_velo = np.matmul(T_m_w, T_w_velo)
occupancymap = mapping.occupancymap(scans, T_m_velo, mapshape,
mapsize)
poleparams = poles.detect_poles(occupancymap, mapsize)
# accuscan = o3.PointCloud()
# for j in range(len(scans)):
# scans[j].transform(T_w_velo[j])
# accuscan.points.extend(scans[j].points)
# o3.draw_geometries([accuscan])
# import ndshow
# ndshow.matshow(occupancymap.transpose([2, 0, 1]))
map = {
'poleparams': poleparams,
'istart': istart[i],
'imid': imid[i],
'iend': iend[i]
}
maps.append(map)
bar.update(i)
np.savez(os.path.join(seqdir, localmapfile), maps=maps)
def trimesh_to_open3d(src):
if isinstance(src, trimesh.Trimesh):
dst = open3d.TriangleMesh()
dst.vertices = open3d.Vector3dVector(src.vertices)
dst.triangles = open3d.Vector3iVector(src.faces)
vertex_colors = np.zeros((len(src.vertices), 3), dtype=float)
for face, face_color in zip(src.faces, src.visual.face_colors):
vertex_colors[face] += face_color[:3] / 255.0 # uint8 -> float
indices, counts = np.unique(src.faces.flatten(), return_counts=True)
vertex_colors[indices] /= counts[:, None]
dst.vertex_colors = open3d.Vector3dVector(vertex_colors)
dst.compute_vertex_normals()
elif isinstance(src, trimesh.PointCloud):
dst = open3d.PointCloud()
dst.points = open3d.Vector3dVector(src.vertices)
if src.colors:
colors = src.colors
colors = (colors[:, :3] / 255.0).astype(float)
dst.colors = open3d.Vector3dVector(colors)
elif isinstance(src, trimesh.scene.Camera):
dst = open3d.PinholeCameraIntrinsic(
width=src.resolution[0],
height=src.resolution[1],
fx=src.K[0, 0],
fy=src.K[1, 1],
cx=src.K[0, 2],
cy=src.K[1, 2],
)
elif isinstance(src, trimesh.path.Path3D):
lines = []
for entity in src.entities:
for i, j in zip(entity.points[:-1], entity.points[1:]):
lines.append((i, j))
lines = np.vstack(lines)
points = src.vertices
dst = open3d.LineSet()
dst.lines = open3d.Vector2iVector(lines)
dst.points = open3d.Vector3dVector(points)
elif isinstance(src, list):
dst = [trimesh_to_open3d(x) for x in src]
else:
raise ValueError("Unsupported type of src: {}".format(type(src)))
return dst
def uniform_downsample(self, voxel_size):
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(copy.deepcopy(self.points))
pcd_open3d = open3d.voxel_down_sample(pcd_open3d,
voxel_size=voxel_size)
self.points = numpy.asarray(pcd_open3d.points)
return
def voxel(data):
mesh = Param["mesh"]
if mesh == 0: return data
if len(data) < 10: return data
d = data.astype(np.float32)
pc = o3d.PointCloud()
pc.points = o3d.Vector3dVector(d)
dwpc = o3d.voxel_down_sample(pc, voxel_size=mesh)
return np.reshape(np.asarray(dwpc.points), (-1, 3))
def np_to_pcd(xyz):
"""
convert numpy array to point cloud object in open3d
"""
xyz = xyz.reshape(-1, 3)
pcd = o3d.PointCloud()
pcd.points = o3d.Vector3dVector(xyz)
pcd.paint_uniform_color(np.random.rand(3, ))
return pcd
def convert(input_dir,
obj_file_name,
mtl_file_name,
ply_file_name,
n,
write_ascii=True):
materials = parse_mtl(mtl_file_name)
vertices, faces, textures, parts = parse_obj(input_dir, obj_file_name,
materials)
points, colors, normals = sample(vertices, faces, textures, parts, n,
input_dir, materials)
pc = open3d.PointCloud()
pc.points = open3d.Vector3dVector(points)
pc.colors = open3d.Vector3dVector(colors)
pc.normals = open3d.Vector3dVector(normals)
open3d.write_point_cloud(ply_file_name, pc, write_ascii=write_ascii)
def downsample_pcd(pcd, voxel_size):
""" Use voxel downsampling from Open3D on PointCloud object """
pcd_open3d = open3d.PointCloud()
pcd_open3d.points = open3d.Vector3dVector(pcd.points)
pcd_open3d = open3d.voxel_down_sample(pcd_open3d, voxel_size=voxel_size)
pcd = PointCloud(points=numpy.asarray(pcd_open3d.points))
return pcd
