def points_spliting(points, pcl_fn):
if NO_SPLIT:
wall_fn = pcl_fn.replace('pcl_camref.ply','object_bbox/wall.txt')
walls = np.loadtxt(wall_fn).reshape([-1,7])
ceil_fn = pcl_fn.replace('pcl_camref.ply','object_bbox/ceiling.txt')
ceilings = np.loadtxt(ceil_fn).reshape([-1,7])
points = forcely_crop_scene(points, walls, ceilings)
points_splited = [points]
else:
splited_vidx, block_size = IndoorData.split_xyz(points[:,0:3],
IndoorData._block_size0.copy(), IndoorData._block_stride_rate,
IndoorData._min_pn_inblock)
if splited_vidx[0] is None:
points_splited = [points]
else:
points_splited = [np.take(points, vidx, axis=0) for vidx in splited_vidx]
pnums0 = [p.shape[0] for p in points_splited]
#print(pnums0)
points_splited = [random_sample_pcl(p, IndoorData._num_points, only_reduce=True)
for p in points_splited]
show = False
if show:
pcds = [points2pcd_open3d(points) for points in points_splited]
#open3d.draw_geometries(pcds)
for pcd in pcds:
open3d.draw_geometries([pcd])
return points_splited
def icp_rabbit(datalist):
A1, A2, A3, A4, A5, A6, A7, A8, A9, A10 = datalist
o3d.draw_geometries([A8, A9])
print("start A3->A2")
A8, R9 = icp(A8, A9)
o3d.draw_geometries([A8, A9])
return
def main():
dm = DataManagement()
after = dt(2018, 7, 23, 14, 0, 0)
before = dt(2018, 7, 23, 14, 1, 0)
datetimes = dm.get_datetimes_in(after, before)
print(datetimes)
assert len(datetimes) == 1
datetime = datetimes[0]
data_path = dm.get_save_directory(datetime)
data_path = os.path.join(data_path, 'objects')
print(os.listdir(data_path))
ob_name = 'keyboard'
ob_path = os.path.join(data_path, ob_name)
# just get one
files = os.listdir(ob_path)
filename = files[0]
csv_path = os.path.join(ob_path, filename)
np_pc = np.loadtxt(csv_path, delimiter=',')
pc = o3.PointCloud()
pc.points = o3.Vector3dVector(np_pc)
o3.draw_geometries([pc])
pass
def draw_open3d(source, target, transformation):
""" Draws the registration result in seperate window with open3d visualizer (GDAL) """
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
if isinstance(source_temp, PointCloud) == True:
pcd_source = open3d.PointCloud()
pcd_source.points = open3d.Vector3dVector(source_temp.points)
elif isinstance(source_temp, numpy.ndarray) == True:
pcd_source = open3d.PointCloud()
pcd_source.points = open3d.Vector3dVector(source_temp)
elif isinstance(source_temp, open3d.PointCloud) == True:
pcd_source = copy.deepcopy(source_temp)
else:
raise TypeError("Point clouds need to be of type (ndarray), (PointCloud) or (open3d.PointCloud)!")
if isinstance(target_temp, PointCloud) == True:
pcd_target = open3d.PointCloud()
pcd_target.points = open3d.Vector3dVector(target_temp.points)
elif isinstance(target_temp, numpy.ndarray) == True:
pcd_target = open3d.PointCloud()
pcd_target.points = open3d.Vector3dVector(target_temp)
elif isinstance(target_temp, open3d.PointCloud) == True:
pcd_target = copy.deepcopy(target_temp)
else:
raise TypeError("Point clouds need to be of type (ndarray) or (PointCloud)!")
# Paint with uniform color
pcd_source.paint_uniform_color([1, 0.706, 0])
pcd_target.paint_uniform_color([0, 0.651, 0.929])
pcd_source.transform(transformation)
open3d.draw_geometries([pcd_source, pcd_target])
return
def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
open3d.draw_geometries([source_temp, target_temp])
def open3dVis(self, depth, normal=None, color=None):
"""Visualize 3d map points from eigen depth
Args:
depth: depth map
normal: normal map
color: rgb image
"""
points = get3dpoints(depth, color)
points = np.asarray(points)
pcd = PointCloud()
pcd.points = Vector3dVector(points)
if color is not None:
color = np.reshape(color, [-1, 3])
pcd.colors = Vector3dVector(color / 255.)
if normal is not None:
normal = np.reshape(normal, [-1, 3])
else:
_, normal, _ = self.compute_local_planes(points[:, 0],
points[:, 1], points[:,
2])
normal = np.reshape(normal, [-1, 3])
pcd.normals = Vector3dVector(normal)
draw_geometries([pcd])
def icp_rabbit(datalist):
A1, A2, A3, A4, A5, A6, A7, A8, A9 = datalist
#o3d.draw_geometries([A1,A2,A3,A4,A5,A6,A7,A8,A9])
train = [A1, A2, A3, A4, A5, A6, A7, A8, A9]
train.remove(A1)
print("start A2->A1")
A2, R1 = icp(A2, A1)
o3d.draw_geometries([A1, A2])
rotate_elements(train.remove(A2), R1)
print("start A3->A2")
A3, R2 = icp(A3, A2)
rotate_elements([A4, A5, A6, A7, A8, A9], R2)
o3d.draw_geometries([A1, A2, A3])
print("start A4->A3")
A4, R3 = icp(A4, A3)
rotate_elements([A5, A6], R3)
o3d.draw_geometries([A1, A2, A3, A4])
print("start A5->A4")
A5, R4 = icp(A5, A4)
rotate_elements([A6], R4)
o3d.draw_geometries([A1, A2, A3, A4, A5])
print("start A6->A5")
A6, R5 = icp(A6, A5)
o3d.draw_geometries([A1, A2, A3, A4, A5, A6])
return
def ViewPLY(path):
# 読み込み
pointcloud = open3d.read_point_cloud(path)
# ダウンサンプリング
# 法線推定できなくなるので不採用
pointcloud = open3d.voxel_down_sample(pointcloud, 10)
# 法線推定
open3d.estimate_normals(pointcloud,
search_param=open3d.KDTreeSearchParamHybrid(
radius=20, max_nn=30))
# 法線の方向を視点ベースでそろえる
open3d.orient_normals_towards_camera_location(pointcloud,
camera_location=np.array(
[0., 10., 10.],
dtype="float64"))
# 可視化
open3d.draw_geometries([pointcloud])
# numpyに変換
points = np.asarray(pointcloud.points)
normals = np.asarray(pointcloud.normals)
print("points:{}".format(points.shape[0]))
X, Y, Z = Disassemble(points)
# OBB生成
_, _, length = buildOBB(points)
print("OBB_length: {}".format(length))
return points, X, Y, Z, normals, length
def visualize_pc(*args):
"""
Visualize of list of point clouds
:param *args: a list of Open3D objects, that are point clouds
"""
point_clouds = [points_to_pc(obj) for obj in args]
op3.draw_geometries([*point_clouds])
def main():
system('') # Enable VT100 Emulation for colors in Windows
# color_cube.py header modified so that Open3D can read the colors (eg. 'diffuse_red' -> 'r')
mesh = open3d.read_triangle_mesh('input/extra/color_cube_mod.ply')
he_mesh = HalfEdgeDS(mesh)
print('Displaying Original Mesh...')
print('Vertices:\x1B[32m', len(he_mesh.vertices), '\x1B[0mEgdes:\x1B[32m',
int(len(he_mesh.halfedges) / 2), '\x1B[0mFaces:\x1B[32m',
len(he_mesh.faces), '\x1B[0m')
mesh.compute_vertex_normals()
open3d.draw_geometries([mesh])
for i in range(5):
print('Deleting random edge.')
deletion_index = randint(0, len(he_mesh.halfedges) - 1)
delete_halfedge(he_mesh, he_mesh.halfedges[deletion_index])
print('Displaying Mesh with {} random edge(s) deleted...'.format(i +
1))
print('Vertices:\x1B[32m',
len(he_mesh.vertices), '\x1B[0mEgdes:\x1B[32m',
int(len(he_mesh.halfedges) / 2), '\x1B[0mFaces:\x1B[32m',
len(he_mesh.faces), '\x1B[0m')
mesh = he_mesh.convert_to_IFS()
mesh.compute_vertex_normals()
if (i == 0):
open3d.write_triangle_mesh('output/hw3p5a.ply', mesh)
open3d.draw_geometries([mesh])
def main():
depth_paths, T, pose_paths = getData(args.shapeid)
n = len(depth_paths)
print('found %d clean depth images...' % n)
intrinsic = np.array([[525.0,0,319.5],[0,525.0,239.5],[0,0,1]])
np.random.seed(816)
indices = np.random.permutation(n)
print(indices[:100])
#indices = sorted(indices)
make_dirs(PATH_MAT.format(args.shapeid, 0))
import open3d
pcd_combined = open3d.PointCloud()
for i, idx in enumerate(indices):
import ipdb; ipdb.set_trace()
print('%d / %d' % (i, len(indices)))
mesh = Mesh.read(depth_paths[idx], mode='depth', intrinsic = intrinsic)
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(mesh.vertex.T)
pcd.transform(inverse(T[idx]))
#pcd = open3d.voxel_down_sample(pcd, voxel_size=0.02)
pcd_combined += pcd
pcd_combined = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
sio.savemat(PATH_MAT.format(args.shapeid, i), mdict={
'vertex': mesh.vertex,
'validIdx_rowmajor': mesh.validIdx,
'pose': T[idx],
'depth_path': depth_paths[idx],
'pose_path': pose_paths[idx]})
if i <= 50 and i >= 40:
pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
open3d.draw_geometries([pcd_combined_down])
pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
open3d.draw_geometries([pcd_combined_down])
def draw_all_keypoints(
point_cloud,
keypoint, # type: np.ndarray,
keypoint_size=0.01):
"""
Draw all the keypoint associated with the point_cloud (mesh)
:param point_cloud: open3d point cloud
:param keypoint: np.ndarray in the shape of (3, n_keypoint)
:param keypoint_size: the radius of the keypoint
:return: None
"""
n_keypoint = keypoint.shape[1]
geometries = []
geometries.append(point_cloud)
for i in range(n_keypoint):
# Type conversion
keypoint_i = []
for k in range(3):
keypoint_i.append(keypoint[k, i])
# Insert into geometries
geometries.append(get_keypoint_sphere(keypoint_i, keypoint_size))
# The drawing command
open3d.draw_geometries(geometries)
def view_local_maps(seq):
sequence = dataset.sequence(seq)
seqdir = os.path.join(result_dir, '{:03d}'.format(seq))
with np.load(os.path.join(seqdir, localmapfile),
allow_pickle=True) as data:
for i, map in enumerate(data['maps']):
T_w_m = sequence.poses[map['imid']].dot(T_cam0_mc).dot(T_mc_m)
mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0])
mapboundsvis.transform(T_w_m)
polemap = []
for poleparams in map['poleparams']:
x, y, zs, ze, a, _ = poleparams
pole = util.create_wire_box([a, a, ze - zs],
color=[1.0, 1.0, 0.0])
T_m_p = np.identity(4)
T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs]
pole.transform(T_w_m.dot(T_m_p))
polemap.append(pole)
accucloud = o3.PointCloud()
for j in range(map['istart'], map['iend']):
velo = sequence.get_velo(j)
cloud = o3.PointCloud()
cloud.points = o3.Vector3dVector(velo[:, :3])
cloud.colors = o3.Vector3dVector(
util.intensity2color(velo[:, 3]))
cloud.transform(sequence.poses[j].dot(
sequence.calib.T_cam0_velo))
accucloud.points.extend(cloud.points)
accucloud.colors.extend(cloud.colors)
o3.draw_geometries([accucloud, mapboundsvis] + polemap)
def plot_point_cloud(pts4D, colours4D=[], verbose=False):
"""Create a point cloud of all 3D points found so far
:param pts4D: a list of 4D (homogeneous) points to be plotted
:param colours4D: a list of colour extracted from the image for each
point in pts4D. Must be the same length as pts4D.
:param verbose: as in pipeline()
"""
# convert from homogeneous coordinates to 3D
pts3D = pts4D[:, :3] / np.repeat(pts4D[:, 3], 3).reshape(-1, 3)
# Plot with open3d
plot_list = []
if len(colours4D) == 0:
# Plot all points red
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(pts3D)
pcd.paint_uniform_color([1, 0, 0])
plot_list.append(pcd)
# If colours are given, use these
else:
# Plot each point with its given colour
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(pts3D)
pcd.colors = open3d.Vector3dVector(colours4D)
plot_list.append(pcd)
# Print the number of 3D points plotted
print("Plotted {} 3D points".format(len(pts3D)))
# Draw the point cloud
open3d.draw_geometries(plot_list)
def visualize_correspondence_smoothness(points, correspondence, max_error, min_error):
""" Visualize smoothness of predicted correspondences
Args:
points: o3d.geometry.TriangleMesh, contains N points.
correspondence: [N]
"""
pcd = getPointCloud(points)
model = helper.loadSMPLModels()[0]
pcd.points = o3d.utility.Vector3dVector(np.array(pcd.points)+np.array([1.0,0,0]).reshape((1, 3)))
v = np.array(pcd.points)
N = v.shape[0]
tree = NN(n_neighbors=20).fit(v)
dists, indices = tree.kneighbors(v)
target = model.verts[correspondence, :] # [N, 3]
centers = target[indices, :].mean(axis=1) # [N, 3]
diff_norms = np.square(target[indices, :] - centers[:, np.newaxis, :]).sum(axis=-1).mean(axis=1) # [N]
diff_norms[np.where(diff_norms > max_error)] = max_error
diff_norms[np.where(diff_norms < min_error)] = min_error
import ipdb; ipdb.set_trace()
max_indices = np.argsort(diff_norms)[-1000:]
edges = getEdges(v[max_indices, :], target[max_indices, :])
colors = (diff_norms-min_error)/(max_error - min_error)
r = np.outer(np.ones(N), np.array([1.0, 0, 0])) # [N, 3]
b = np.outer(np.ones(N), np.array([0., 0, 1.0])) # [N, 3]
colors = b*(1.0-colors[:, np.newaxis])+r*(colors[:, np.newaxis])
pcd.points = o3d.utility.Vector3dVector(colors)
o3d.draw_geometries([pcd, getTriangleMesh(model.verts, model.faces), edges])
def draw_registration_result(source, target):
if isinstance(source, PointCloud):
tmp = source
source = o3d.PointCloud()
source.points = o3d.Vector3dVector(tmp.position)
elif isinstance(source, np.ndarray):
tmp = source
source = o3d.PointCloud()
source.points = o3d.Vector3dVector(tmp)
if isinstance(target, PointCloud):
tmp = target
target = o3d.PointCloud()
target.points = o3d.Vector3dVector(tmp.position)
elif isinstance(target, np.ndarray):
tmp = target
target = o3d.PointCloud()
target.points = o3d.Vector3dVector(tmp)
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
o3d.draw_geometries([source_temp, target_temp],
window_name='Open3D',
width=1920,
height=1080,
left=0,
top=0)
def view_local_maps(sessionname):
sessiondir = os.path.join(pynclt.resultdir, sessionname)
session = pynclt.session(sessionname)
maps = np.load(os.path.join(sessiondir, get_localmapfile()))['maps']
for i, map in enumerate(maps):
print('Map #{}'.format(i))
mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0])
mapboundsvis.transform(map['T_w_m'])
polevis = []
for poleparams in map['poleparams']:
x, y, zs, ze, a = poleparams[:5]
pole = util.create_wire_box(
[a, a, ze - zs], color=[1.0, 1.0, 0.0])
T_m_p = np.identity(4)
T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs]
pole.transform(map['T_w_m'].dot(T_m_p))
polevis.append(pole)
accucloud = o3.PointCloud()
for j in range(map['istart'], map['iend']):
points, intensities = session.get_velo(j)
cloud = o3.PointCloud()
cloud.points = o3.Vector3dVector(points)
cloud.colors = o3.Vector3dVector(
util.intensity2color(intensities / 255.0))
cloud.transform(session.T_w_r_odo_velo[j])
accucloud.points.extend(cloud.points)
accucloud.colors.extend(cloud.colors)
o3.draw_geometries([accucloud, mapboundsvis] + polevis)
def draw_registration_result(src, trgt):
source = open3d.geometry.PointCloud()
source.points = open3d.utility.Vector3dVector(src)
target = open3d.geometry.PointCloud()
target.points = open3d.utility.Vector3dVector(trgt)
source.paint_uniform_color([1, 0.706, 0])
target.paint_uniform_color([0, 0.651, 0.929])
open3d.draw_geometries([source, target])
def compare_with_room(self):
pc_room = o3.read_point_cloud('static_data/room_A.ply')
pcd = self.get_pcd()
P = np.loadtxt('static_data/T.csv', delimiter=',')
pcd.transform(P)
o3.draw_geometries([pc_room, pcd])
def color_deviations(config, result, distances):
""" Use the Open3d visualization function to colorize deviations after convergence """
if isinstance(result, PointCloud) == True:
result = open3d.PointCloud()
result.points = open3d.Vector3dVector(result.points)
elif isinstance(result, numpy.ndarray) == True:
result = open3d.PointCloud()
result.points = open3d.Vector3dVector(result)
elif isinstance(result, open3d.PointCloud) == True:
pass
else:
raise TypeError(
"Point clouds need to be of type (ndarray), (PointCloud) or (open3d.PointCloud)!"
)
# Needed Paths
OPEN3D_BUILD_PATH = "/home/chris/Documents/Masterarbeit/Code/virt_env/env/lib/python3.6/site-packages/Open3D/build/"
OPEN3D_EXPERIMENTAL_BIN_PATH = OPEN3D_BUILD_PATH + "/bin/Experimental/"
# TODO use os.findpath or something to make this dynamic
DATASET_DIR = "/home/chris/Documents/Masterarbeit/Code/virt_env/env/ICP/src/bin"
# Log files
MY_LOG_POSTFIX = "_COLMAP_SfM.log"
MY_RECONSTRUCTION_POSTFIX = "_COLMAP.ply"
# File structure
mvs_outpath = DATASET_DIR + config["colorize_deviations"]["output_path"]
"/../../data/set1/output/evaluation/"
scene = config["colorize_deviations"]["scene"] + "/"
# Make directory
make_dir(mvs_outpath)
make_dir(mvs_outpath + "/" + scene)
# New log files
new_logfile = DATASET_DIR + MY_LOG_POSTFIX
colmap_ref_logfile = DATASET_DIR + scene + "_COLMAP_SfM.log"
mvs_file = mvs_outpath + scene + MY_RECONSTRUCTION_POSTFIX
# Write the distances to bin files
numpy.array(distances).astype("float64").tofile(mvs_outpath + "/" + scene +
".precision.bin")
# Colorize the poincloud files with the precision and recall values
open3d.write_point_cloud(mvs_outpath + "/" + scene + ".precision.ply",
result)
open3d.write_point_cloud(mvs_outpath + "/" + scene + ".precision.ncb.ply",
result)
result_n_fn = mvs_outpath + "/" + scene + ".precision.ply"
eval_str_viewDT = OPEN3D_EXPERIMENTAL_BIN_PATH + "ViewDistances " + result_n_fn + " --max_distance " + str(
numpy.asarray(distances).max()) + " --write_color_back --without_gui"
os.system(eval_str_viewDT)
result_colored = open3d.read_point_cloud(result_n_fn)
open3d.draw_geometries([result_colored])
return
def visualize_wireframe(annos):
"""visualize wireframe
"""
colormap = np.array(colormap_255) / 255
junctions = np.array([item['coordinate'] for item in annos['junctions']])
_, junction_pairs = np.where(np.array(annos['lineJunctionMatrix']))
junction_pairs = junction_pairs.reshape(-1, 2)
# extract hole lines
lines_holes = []
for semantic in annos['semantics']:
if semantic['type'] in ['window', 'door']:
for planeID in semantic['planeID']:
lines_holes.extend(
np.where(np.array(
annos['planeLineMatrix'][planeID]))[0].tolist())
lines_holes = np.unique(lines_holes)
# extract cuboid lines
cuboid_lines = []
for cuboid in annos['cuboids']:
for planeID in cuboid['planeID']:
cuboid_lineID = np.where(
np.array(annos['planeLineMatrix'][planeID]))[0].tolist()
cuboid_lines.extend(cuboid_lineID)
cuboid_lines = np.unique(cuboid_lines)
cuboid_lines = np.setdiff1d(cuboid_lines, lines_holes)
# visualize junctions
connected_junctions = junctions[np.unique(junction_pairs)]
connected_colors = np.repeat(colormap[0].reshape(1, 3),
len(connected_junctions),
axis=0)
junction_set = open3d.PointCloud()
junction_set.points = open3d.Vector3dVector(connected_junctions)
junction_set.colors = open3d.Vector3dVector(connected_colors)
# visualize line segments
line_colors = np.repeat(colormap[5].reshape(1, 3),
len(junction_pairs),
axis=0)
# color holes
if len(lines_holes) != 0:
line_colors[lines_holes] = colormap[6]
# color cuboids
if len(cuboid_lines) != 0:
line_colors[cuboid_lines] = colormap[2]
line_set = open3d.LineSet()
line_set.points = open3d.Vector3dVector(junctions)
line_set.lines = open3d.Vector2iVector(junction_pairs)
line_set.colors = open3d.Vector3dVector(line_colors)
open3d.draw_geometries([junction_set, line_set])
def main():
pc_files = ["input/bunny-pts.ply",
"input/mobius_3t.xyz"] # files storing point cloud data
for i in range(2):
print("\nReading and Displaying", pc_files[i])
pcd = open3d.read_point_cloud(
pc_files[i]) # read point cloud data from file
print(pcd) # print details of point cloud
open3d.draw_geometries([pcd]) # display point cloud
def np_to_pcd(np_pc, draw=False):
pcd = open3d.PointCloud()
pcd.points = open3d.Vector3dVector(np_pc[:, :3])
pcd.colors = open3d.Vector3dVector(np_pc[:, 3:] / 255.0)
if draw:
open3d.draw_geometries([pcd])
return pcd
def visualize_fitting(model, raw): pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(raw) pcd.paint_uniform_color(color(0)) mesh = o3d.geometry.TriangleMesh() mesh.vertices = o3d.utility.Vector3dVector(model.verts) mesh.triangles = o3d.utility.Vector3iVector(model.faces) o3d.draw_geometries([mesh, pcd])
def plot_flow(x, t, phi, grid_size, t_sample):
"""
Plot the ground truth points, and the reconstructed patch by meshing the domain [0, 1]^2 and lifting the mesh
to R^3
:param x: The ground truth points we are trying to fit
:param t: The sample positions used to fit the mesh
:param phi: The fitted neural network
:param grid_size: The number of sample positions per axis in the meshgrid
:return: None
"""
# I'm doing the input here so you don't crash if you never use this function and don't have OpenGL
import open3d
with torch.no_grad():
mesh_samples = embed_3d(
torch.from_numpy(meshgrid_vertices(grid_size)).to(x), t[0, 2])
mesh_faces = meshgrid_face_indices(grid_size)
mesh_vertices = phi(mesh_samples)[:, 0:3]
recon_vertices = phi(t)[:, 0:3]
gt_color = np.array([0.1, 0.7, 0.1])
recon_color = np.array([0.7, 0.1, 0.1])
mesh_color = np.array([0.1, 0.1, 0.7])
curve_color = np.array([0.2, 0.2, 0.5])
pcloud_gt = open3d.PointCloud()
pcloud_gt.points = open3d.Vector3dVector(phi.invert(x).cpu().numpy())
pcloud_gt.paint_uniform_color(gt_color)
pcloud_recon = open3d.PointCloud()
pcloud_recon.points = open3d.Vector3dVector(
recon_vertices.cpu().numpy())
pcloud_recon.paint_uniform_color(recon_color)
mesh_recon = open3d.TriangleMesh()
mesh_recon.vertices = open3d.Vector3dVector(
mesh_vertices.cpu().numpy())
mesh_recon.triangles = open3d.Vector3iVector(mesh_faces)
mesh_recon.compute_vertex_normals()
mesh_recon.paint_uniform_color(mesh_color)
pc_initial = open3d.PointCloud()
pc_initial.points = open3d.Vector3dVector(t.cpu().numpy())
pc_initial.paint_uniform_color(curve_color)
flow_ode = open3d.LineSet()
# print(x.shape)
# print(t.shape)
flow = get_Lines(phi, t[::t_sample, :], 15)
flow_ode.points, flow_ode.lines = open3d.Vector3dVector(flow[0]), \
open3d.Vector2iVector(flow[1])
# flow_ode.colors = open3d.Vector3dVector(curve_color)
open3d.draw_geometries(
[pcloud_gt, pcloud_recon, mesh_recon, pc_initial, flow_ode])
def show(xyz, XYZ):
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
obs = [pcd]
for ifor in range(XYZ.shape[0]):
sp = o3d.create_mesh_sphere(0.05).transform(pose(XYZ[ifor, :]))
obs.append(sp)
o3d.draw_geometries(obs)
def draw_registration_result(source, target, transformation):
# display results having two point clouds and transformation for source
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
mesh_frame = o3d.create_mesh_coordinate_frame(size = 200, origin = [0, 0, 0])
o3d.draw_geometries([source_temp, target_temp, mesh_frame])
def visualize_points_with_labels(points, labels, cmap=None, lut=6):
if cmap is None:
cmap = plt.get_cmap("hsv", lut)
cmap = np.asarray([cmap(i) for i in range(lut)])[:, :3]
point_cloud = open3d.PointCloud()
point_cloud.points = open3d.Vector3dVector(points)
assert len(labels) == len(points)
point_cloud.colors = open3d.Vector3dVector(cmap[labels])
open3d.draw_geometries([point_cloud])
def show_mesh(vertices, triangle, color=[0, 0, 0], only_genmesh=False):
mesh = open3d.TriangleMesh()
mesh.vertices = open3d.Vector3dVector(vertices)
mesh.triangles = open3d.Vector3iVector(triangle)
mesh.paint_uniform_color(color)
centroid = np.mean(vertices, 0)
mesh_frame = open3d.create_mesh_coordinate_frame(size=1.6, origin=centroid)
if not only_genmesh:
open3d.draw_geometries([mesh, mesh_frame])
return mesh
def save_local_maps(sessionname, visualize=False):
print(sessionname)
session = pynclt.session(sessionname)
util.makedirs(session.dir)
istart, imid, iend = get_map_indices(session)
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])):
xyz, _ = session.get_velo(val)
scan = o3.geometry.PointCloud()
scan.points = o3.utility.Vector3dVector(xyz)
scans.append(scan)
T_w_mc = util.project_xy(
session.T_w_r_odo_velo[imid[i]].dot(T_r_mc))
T_w_m = T_w_mc.dot(T_mc_m)
T_m_w = util.invert_ht(T_w_m)
T_w_r = session.T_w_r_odo_velo[istart[i]:iend[i]]
T_m_r = np.matmul(T_m_w, T_w_r)
occupancymap = mapping.occupancymap(scans, T_m_r, mapshape,
mapsize)
poleparams = poles.detect_poles(occupancymap, mapsize)
if visualize:
cloud = o3.geometry.PointCloud()
for T, scan in zip(T_w_r, scans):
s = copy.copy(scan)
s.transform(T)
cloud.points.extend(s.points)
mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0])
mapboundsvis.transform(T_w_m)
polevis = []
for j in range(poleparams.shape[0]):
x, y, zs, ze, a = poleparams[j, :5]
pole = util.create_wire_box([a, a, ze - zs],
color=[1.0, 1.0, 0.0])
T_m_p = np.identity(4)
T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs]
pole.transform(T_w_m.dot(T_m_p))
polevis.append(pole)
o3.draw_geometries(polevis + [cloud, mapboundsvis])
map = {
'poleparams': poleparams,
'T_w_m': T_w_m,
'istart': istart[i],
'imid': imid[i],
'iend': iend[i]
}
maps.append(map)
bar.update(i)
np.savez(os.path.join(session.dir, get_localmapfile()), maps=maps)
