def custom_draw_geometry_with_rotation(pcd):
def rotate_view(vis):
ctr = vis.get_view_control()
ctr.rotate(5.0, 5.0)
return False
draw_geometries_with_animation_callback(pcd, rotate_view)
def custom_draw_geometry_with_view_tracking(meshes):
def track_view(vis):
global i, A1, A2
for i in range(30):
p1 = np.array(A1.points)
p2 = np.array(A2.points)
p1, p2, indice = point_matching(p1, p2)
print("[" + str(i) + "/91] loss:" + str(icploss(p1, p2)))
weight = np.linalg.norm((p1-p2),axis=1)
weight = (1e-1/(weight+1e-1))[:,np.newaxis]
Tran0 = cal_transformation(p1, p2,weight) #如果不需要ilrs不要输入 #p2p
#p2norm_use = p2norm [indice] #p2pl
#Tran0 = cal_transformation_p2pl(p1, p2,p2norm_use) #p2pl
A1.transform(Tran0)
i += 1
vis.update_geometry()
o3d.draw_geometries_with_animation_callback(meshes, track_view)
def custom_draw_geometry_with_rotation(pcd):
def rotate_view(vis):
ctr = vis.get_view_control()
ctr.rotate(10.0, 0.0)
global i, images
i += 1
print i % 210, i // 210
image = np.asarray(vis.capture_screen_float_buffer())
image = np.array(255 * image, dtype=np.uint8)
image = imresize(image, 0.25)
if (i // 210 == 0):
images.append(image)
return False
open3d.draw_geometries_with_animation_callback([pcd], rotate_view)
def draw_bundles(bundles_list, colour_list=[], rotate=False):
bundles = []
for idx, bundle in enumerate(bundles_list):
if len(colour_list) < 1:
colour_list = [[random(), random(), random()]
for _ in range(len(bundles_list))]
assert len(bundles_list) == len(colour_list)
colour = colour_list[idx]
for points in bundle:
lines = [[i, i + 1] for i in range(len(points) - 1)]
data_line = LineSet()
data_line.points = Vector3dVector(points)
data_line.lines = Vector2iVector(lines)
data_line.colors = Vector3dVector(
[colour for _ in range(len(lines))])
bundles.append(data_line)
if rotate:
draw_geometries_with_animation_callback(bundles, __rotate_view)
else:
draw_geometries(bundles)
def custom_draw_geometry_with_view_tracking(meshes):
def track_view(vis):
global i, A1, A2
if i % 10 == 0:
point1 = np.array(A1.points)
point2 = np.array(A2.points)
p1, p2, indice = point_matching(point1,
point2) #, color1, color2, 1)
weight = np.linalg.norm((p1 - p2), axis=1)
avg_weight = 0.45 #np.average(weight)
#print(avg_weight)
weight = np.exp(3 * (avg_weight /
(weight + avg_weight))[:, np.newaxis])
print("loss" + str(i) + ":" + str(icploss(p1, p2, weight)))
Trans = cal_transformation(p1, p2, weight) # point 2 point
# p2norm_use = p2norm[indice] #point 2 plane
# Trans = cal_transformation_p2pl(p1, p2, p2norm_use) #point 2 plane
A1.transform(Trans)
vis.update_geometry()
i += 1
o3d.draw_geometries_with_animation_callback(meshes, track_view)
def custom_draw_geometry_with_view_tracking(meshes):
def track_view(vis):
global matchlist1, pointlist1, INDEX, opt, pose, E0, Intrinsic
if (INDEX % 1000 == 0):
# reverse
# import pdb
# pdb.set_trace()
#print(E0)
print("--------------")
#################
verse = np.eye(4)
verse[:3, :3] = np.linalg.inv(pose[:3, :3])
#arrow2.transform(pose) # 变回去
#################
E = []
for i in range(len(matchlist1)):
E.append(E0.copy())
# print("***",matchlist1,pointlist1)
print(
"interation", str(INDEX / 1000), "--loss:",
opt.loss(np.zeros((1, 3)), np.zeros((1, 3)), Intrinsic, E,
matchlist1, pointlist1)[0][0])
E = opt.image_R(E, matchlist1, pointlist1, Intrinsic)
E = opt.image_t(E, matchlist1, pointlist1, Intrinsic)
###################
pose = np.eye(4)
pose[:3, :3] = E[0][:3, :3]
###################
print(E[0])
E0 = E[0].copy()
vis.update_geometry()
INDEX += 1
o3d.draw_geometries_with_animation_callback(meshes, track_view)
def custom_draw_geometry_with_view_tracking(mesh, generate, period):
def track_view(vis):
global frame, poses
global num_frames, last_T
global parameter
ctr = vis.get_view_control()
if frame == 0:
params = ctr.convert_to_pinhole_camera_parameters()
intrinsics = params.intrinsic
extrinsics = params.extrinsic
pose = np.array(
[[-0.8188861922, 0.3889273405, -0.4220911372, -14.6068376600],
[-0.1157361687, -0.8321937190, -0.5422718444, 23.0477832143],
[-0.5621659395, -0.3952077147, 0.7264849060, 4.1193224787],
[0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000]])
params = o3d.camera.PinholeCameraParameters()
params.extrinsic = pose
params.intrinsic = intrinsics
ctr.convert_from_pinhole_camera_parameters(params)
fid = frame % num_frames
keyindex1 = 500
keyindex2 = 600
if True:
intrinsics = o3d.read_pinhole_camera_intrinsic(
"stream/%d.intrinsics.json" % fid)
T = np.loadtxt('stream/%d.pose' % fid)
params = o3d.camera.PinholeCameraParameters()
params.extrinsic = T
params.intrinsic = intrinsics
ctr.convert_from_pinhole_camera_parameters(params) #很重要，不能删除
if (fid == keyindex1) | (fid == keyindex2):
""" Generate Point Cloud """
if (last_T is None) or ((fid in frame_trace) or
(AngularDistance(T, last_T))):
print('%d/%d' % (fid, num_frames))
depth = vis.capture_depth_float_buffer(False)
depth = np.array(depth)
idx = np.where(depth > 30)
depth[idx] = 0
depth = o3d.Image(depth)
image = vis.capture_screen_float_buffer(False)
image = o3d.Image(
np.array(np.array(image) * 255).astype(np.uint8))
rgbd = o3d.create_rgbd_image_from_color_and_depth(
image, depth, convert_rgb_to_intensity=False)
rgbd.depth = o3d.Image(np.array(rgbd.depth) * 1000)
pcd = o3d.create_point_cloud_from_rgbd_image(
rgbd, intrinsics)
pcd.transform(np.linalg.inv(T))
o3d.write_point_cloud("stream/%d.ply" % fid,
pcd,
write_ascii=True)
cv2.imwrite("stream/%d.png" % fid, np.array(image))
depth = np.array(depth)
#depth = np.array(depth)*1000
#depth.astype(np.uint16)
#cv2.imwrite("stream/%d_depth.png" % fid, depth)##
last_T = T
if (fid == keyindex1):
intrinsic_matrix = intrinsics.intrinsic_matrix
pcd0, image, indexlist = see2pcd(
fid, depth, np.array(image), pcd,
intrinsics.intrinsic_matrix)
pcd0.transform(np.linalg.inv(T))
#print(T)
parameter['0'] = [pcd0, image, indexlist]
if (fid == keyindex2):
intrinsic_matrix = intrinsics.intrinsic_matrix
pcd1, image, indexlist = see2pcd(
fid, depth, np.array(image), pcd,
intrinsics.intrinsic_matrix)
pcd1.transform(np.linalg.inv(T))
#print(T)
parameter['1'] = [pcd1, image, indexlist]
vis.destroy_window()
#o3d.draw_geometries([parameter['0'][0],pcd1])
compare(parameter)
os.system("pause")
if fid == num_frames - 1:
exit()
frame += 1
o3d.draw_geometries_with_animation_callback([mesh, pcd_partial],
track_view,
width=1920,
height=1080)
fused=True)
inputs = tf.nn.sigmoid(inputs)
return inputs[:, :1], inputs[:, 1:]
inputs = tf.placeholder(tf.float32, [None, 3])
outputs = generator(inputs, stddev=0.03)
with tf.Session() as session:
session.run(tf.global_variables_initializer())
mesh_sphere = open3d.create_mesh_sphere(radius=1.0, resolution=1000)
vertices = np.array(mesh_sphere.vertices)
scale, colors = session.run(outputs, feed_dict={inputs: vertices})
mesh_sphere.vertices = open3d.Vector3dVector(vertices * scale)
mesh_sphere.vertex_colors = open3d.Vector3dVector(colors)
mesh_sphere.compute_vertex_normals()
def call_back(visualizer):
visualizer.get_render_option().background_color = np.asarray([0, 0, 0])
visualizer.get_view_control().rotate(10.0, 0.0)
return False
open3d.draw_geometries_with_animation_callback([mesh_sphere], call_back)
def custom_draw_geometry_with_view_tracking(mesh):
def track_view(vis):
global frame, poses
global num_frames, last_T
#print('frame=%d' % frame)
#import ipdb; ipdb.set_trace()
ctr = vis.get_view_control()
######intrinsics = o3d.read_pinhole_camera_intrinsic('intrinsics.json')
#intrinsics = o3d.PinholeCameraIntrinsic(o3d.PinholeCameraIntrinsicParameters.Kinect2ColorCameraDefault)
######intrinsics = o3d.PinholeCameraIntrinsic(
###### o3d.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
######intrinsics_mat=np.array([[935.30743608719376, 0.0, 0.0],
###### [0.0, 935.30743608719376, 0.0],
###### [959.5, 539.5, 1.0]])
######intrinsics.intrinsic_matrix = intrinsics_mat
#import ipdb; ipdb.set_trace()
######print(intrinsics)
#pcd = o3d.create_point_cloud_from_rgbd_image(rgbd, intrinsics)
#o3d.write_point_cloud("stream/%d.ply" % frame, pcd)
if frame == 0:
#vis.get_render_option().load_from_json("render.json")
intrinsics, extrinsics = ctr.convert_to_pinhole_camera_parameters()
pose = [[-0.8188861922, 0.3889273405, -0.4220911372, -14.6068376600],
[-0.1157361687, -0.8321937190, -0.5422718444, 23.0477832143],
[-0.5621659395, -0.3952077147, 0.7264849060, 4.1193224787],
[0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000]]
ctr.convert_from_pinhole_camera_parameters(intrinsics, pose)
fid = frame % num_frames
if not args.load:
intrinsics, extrinsics = ctr.convert_to_pinhole_camera_parameters()
o3d.write_pinhole_camera_intrinsic("stream/%d.intrinsics.json" % frame, intrinsics)
with open('stream/%d.pose' % frame, 'w') as fout:
for i in range(4):
for j in range(4):
if j > 0:
fout.write(' ')
fout.write('%.10f' % extrinsics[i, j])
fout.write('\n')
#o3d.write_pinhole_camera_intrinsics(intrinsics, "stream/%d.intrinsics.json" % frame)
#pose_dict['intrinsics'].append(intrinsics)
#pose_dict['extrinsics'].append(extrinsics)
#if frame == 100:
# import pickle
# with open('stream/pose.p', 'wb') as fout:
# pickle.dump(pose_dict, fout, protocol=pickle.HIGHEST_PROTOCOL)
# exit()
else:
intrinsics = o3d.read_pinhole_camera_intrinsic("stream/%d.intrinsics.json" % fid)
T = np.loadtxt('stream/%d.pose' % fid)
ctr.convert_from_pinhole_camera_parameters(intrinsics, T)
if args.generate:
""" Generate Point Cloud """
if (last_T is None) or (fid in frame_trace) or (AngularDistance(T, last_T)):
print('%d/%d' % (fid, num_frames))
#vis.update_geometry()
#vis.poll_events()
#vis.update_renderer()
depth = vis.capture_depth_float_buffer(False)
depth = np.array(depth)
#print(depth.max(), depth.min())
idx = np.where(depth > 30)
depth[idx] = 0
depth = o3d.Image(depth)
image = vis.capture_screen_float_buffer(False)
image = o3d.Image(np.array(np.array(image)*255).astype(np.uint8))
rgbd = o3d.create_rgbd_image_from_color_and_depth(
image, depth, convert_rgb_to_intensity = False)
rgbd.depth = o3d.Image(np.array(rgbd.depth)*1000)
pcd = o3d.create_point_cloud_from_rgbd_image(rgbd, intrinsics)
#pcd = o3d.voxel_down_sample(pcd, voxel_size=0.05)
#pcd.transform(np.linalg.inv(T))
o3d.write_point_cloud("stream/%d.ply" % fid, pcd, write_ascii=True)
cv2.imwrite("stream/%d.png" % fid, np.array(image))
last_T = T
if fid == num_frames - 1:
exit()
frame += 1
o3d.draw_geometries_with_animation_callback([mesh, pcd_partial], track_view)
pointcloud.paint_uniform_color(
utils.item_in_range_color(snapshot_index, 10, True))
transformation = transformations[transformations.files[sensor_index]]
pointcloud.transform(transformation)
snapshot_pointcloud = utils.merge_pointclouds(snapshot_pointcloud,
pointcloud)
return snapshot_pointcloud
def update(vis=None):
global all_snapshots
snapshot_index = check_for_new_images()
if snapshot_index == -1:
return False
snapshot_pointcloud = process_new_snapshot(snapshot_index)
if snapshot_pointcloud is None:
return False
all_snapshots.append(snapshot_pointcloud)
if vis is not None:
vis.add_geometry(snapshot_pointcloud)
vis.update_geometry()
vis.update_renderer()
return False
# Main script:
all_snapshots = []
update()
open3d.draw_geometries_with_animation_callback(all_snapshots, update)