-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathvis_sequence.py
204 lines (169 loc) · 8.22 KB
/
vis_sequence.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
import os
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('--gpu', type=int, default=0)
parser.add_argument('--point_size', type=int, default=512)
parser.add_argument('--min_octree_threshold', type=float, default=0.04)
parser.add_argument('--max_octree_threshold', type=float, default=0.15)
parser.add_argument('--interval_size', type=float, default=0.035)
parser.add_argument('--weight2d_path', type=str, default="weight/model2d.pth")
parser.add_argument('--weight3d_path', type=str, default="weight/model3d.pth")
parser.add_argument('--scene_path', type=str, default="data/scene_0.h5")
parser.add_argument('--use_vis', type=int, default="1")
opt = parser.parse_args()
os.environ['CUDA_VISIBLE_DEVICES'] = str(opt.gpu)
from utils.vis_utils import vis_pointcloud,Vis_color
import torch
import time
from model.model2d import FuseNet_feature
from model.model3d import create_FusionAwareFuseConv
from GLtree.interval_tree import RedBlackTree, Node, BLACK, RED, NIL
from GLtree.octree import point3D
import numpy as np
from utils.ply_utils import write_ply,create_color_palette,label_mapper
import torchvision.transforms as transforms
import random
import h5py
SCANNET_TYPES = {'scannet': (40, [0.496342, 0.466664, 0.440796], [0.277856, 0.28623, 0.291129])}
transform_image = transforms.Compose([transforms.ToTensor(),
transforms.Normalize(SCANNET_TYPES['scannet'][1], SCANNET_TYPES['scannet'][2])])
num_classes = 20
near_node_num = 8
max_node = 8
point_size = opt.point_size
print("[INFO] load model")
model2d=FuseNet_feature(num_classes)
model3d = create_FusionAwareFuseConv(num_classes)
model3d.load_state_dict(torch.load(opt.weight3d_path))
model2d.load_state_dict(torch.load(opt.weight2d_path))
model2d = model2d.cuda()
model3d = model3d.cuda()
color_map=create_color_palette()
model2d.eval()
model3d.eval()
print("[INFO] load data")
data_file=h5py.File(opt.scene_path,"r")
color_image_array=data_file['color_image']
depth_map_array=data_file['depth_map']
valid_pose_array=data_file['pose_valid']
points_array=data_file['points_array']
mask_array=data_file['mask']
x_rb_tree = RedBlackTree(opt.interval_size)
y_rb_tree = RedBlackTree(opt.interval_size)
z_rb_tree = RedBlackTree(opt.interval_size)
vis_p=vis_pointcloud(opt.use_vis)
vis_c=Vis_color(opt.use_vis)
frame_index=0
print("[INFO] begin")
with torch.no_grad():
for i in range(0,color_image_array.shape[0]):
print("---------------------------")
print("image:",i)
time_s=time.time()
color_image=color_image_array[i,:,:,:].astype(np.uint8)
depth_image=depth_map_array[i,:,:]
points=points_array[i,:,:]
points_mask=mask_array[i,:,:]
valid_pose=valid_pose_array[i]
if valid_pose==0:
continue
color_image_cuda = transform_image(color_image).cuda()
depth_image=transforms.ToTensor()(depth_image).type(torch.FloatTensor).cuda()
input_color = torch.unsqueeze(color_image_cuda, 0)
depth_image = torch.unsqueeze(depth_image, 0)
imageft=model2d(input_color,depth_image).detach().cpu().numpy()
x_tree_node_list=[]
y_tree_node_list=[]
z_tree_node_list=[]
per_image_node_set=set()
for p in range(point_size):
x_temp_node = x_rb_tree.add(points[p,0])
y_temp_node = y_rb_tree.add(points[p,1])
z_temp_node = z_rb_tree.add(points[p,2])
x_tree_node_list.append(x_temp_node)
y_tree_node_list.append(y_temp_node)
z_tree_node_list.append(z_temp_node)
for p in range(point_size):
x_set_union = x_tree_node_list[p].set_list
y_set_union = y_tree_node_list[p].set_list
z_set_union = z_tree_node_list[p].set_list
set_intersection = x_set_union[0] & y_set_union[0] & z_set_union[0]
temp_branch = [None, None, None, None, None, None, None, None]
temp_branch_distance = np.full((8),opt.max_octree_threshold)
is_find_nearest = False
branch_record = set()
list_intersection=list(set_intersection)
random.shuffle(list_intersection)
for point_iter in list_intersection:
distance = np.sum(np.absolute(point_iter.point_coor - points[p,:]))
if distance < opt.min_octree_threshold:
is_find_nearest = True
if frame_index!=point_iter.frame_id:
point_iter.feature_fuse = np.maximum(imageft[0, :, int(points_mask[p, 0]),
int(points_mask[p, 1])].copy() , point_iter.feature_fuse)
point_iter.frame_id=frame_index
per_image_node_set.add(point_iter)
break
x = int(point_iter.point_coor[0] >= points[p, 0])
y = int(point_iter.point_coor[1] >= points[p, 1])
z = int(point_iter.point_coor[2] >= points[p, 2])
branch_num= x * 4 + y * 2 + z
if distance < point_iter.branch_distance[7-branch_num]:
branch_record.add((point_iter, 7 - branch_num, distance))
if distance < temp_branch_distance[branch_num]:
temp_branch[branch_num] = point_iter
temp_branch_distance[branch_num] = distance
if not is_find_nearest:
new_3dpoint = point3D(points[p, :].T, imageft[0, :, int(points_mask[p, 0]),
int(points_mask[p, 1])].copy(),opt.max_octree_threshold)
for point_branch in branch_record:
point_branch[0].branch_array[point_branch[1]] = new_3dpoint
point_branch[0].branch_distance[point_branch[1]] = point_branch[2]
new_3dpoint.branch_array = temp_branch
new_3dpoint.branch_distance = temp_branch_distance
per_image_node_set.add(new_3dpoint)
for x_set in x_set_union:
x_set.add(new_3dpoint)
for y_set in y_set_union:
y_set.add(new_3dpoint)
for z_set in z_set_union:
z_set.add(new_3dpoint)
node_lengths=len(per_image_node_set)
input_feature = np.zeros([1, 128, near_node_num, node_lengths])
input_coor = np.zeros([1, 3, near_node_num, node_lengths])
result_feature = np.zeros([1, 128, node_lengths])
points = np.zeros([node_lengths, 3])
points_color = np.zeros([node_lengths,3])
set_count=0
for set_point in per_image_node_set:
neighbor_2dfeature, neighbor_coor,_ =set_point.findNearPoint(near_node_num,max_node)
input_feature[0, :, :, set_count] = neighbor_2dfeature
input_coor[0, :, :, set_count] = neighbor_coor
result_feature[0,:,set_count]=set_point.result_feature
points[set_count,:]=set_point.point_coor
set_count+=1
input_feature=torch.from_numpy(input_feature).cuda()
input_coor=torch.from_numpy(input_coor).cuda()
result_feature=torch.from_numpy(result_feature).cuda()
output,combine_result,uncertainty = model3d(input_feature.float(), input_coor.float(),result_feature.float())
result_array = combine_result.detach().cpu().numpy()
uncertainty_array= uncertainty.detach().cpu().numpy()
point_pred_label=label_mapper[torch.argmax(output, 1).long().squeeze().cpu().numpy()]
set_count=0
for set_point in per_image_node_set:
if uncertainty_array[0][set_count]<set_point.uncertainty:
set_point.result_feature= result_array[0, :, set_count]
set_point.uncertainty=uncertainty_array[0][set_count]
set_point.pred_result=point_pred_label[set_count]
points_color[set_count,:]=color_map[point_pred_label[set_count]]
set_count+=1
frame_index+=1
print("time per frame",time.time()-time_s)
vis_p.update(points,points_color)
vis_c.update(color_image)
point_result=x_rb_tree.all_points_from_tree(return_label=True)
write_ply(point_result[:,:3],label_cloud=point_result[:,3],haslabel=True,output_dir="./",name="result")
del x_rb_tree
del y_rb_tree
del z_rb_tree
vis_p.run()