def forward(self, lats, obs_local, valid_points):
# obs_local: <BxLx2>
self.obs_local = deepcopy(obs_local)
self.valid_points = valid_points
obs = torch.cat([self.obs_local, lats], -1)
self.pose_est = self.loc_net(obs)
self.obs_global_est = transform_to_global_2D(self.pose_est,
self.obs_local)
'''
theta = -65
theta_rad = theta/180*3.1415925
matrix = np.array([[np.cos(theta_rad),-np.sin(theta_rad)],[np.sin(theta_rad),np.cos(theta_rad)]])
matrix_t = torch.from_numpy(matrix).unsqueeze(0).unsqueeze(0).type(torch.FloatTensor).cuda()
matrix_t_r = matrix_t.repeat(self.obs_global_est.size()[0],self.obs_global_est.size()[1],1,1)
matrixl = np.array([[np.cos(theta_rad),-np.sin(theta_rad),0],[np.sin(theta_rad),np.cos(theta_rad),0],[0,0,1]])
matrixl_t = torch.from_numpy(matrixl).unsqueeze(0).type(torch.FloatTensor).cuda()
matrixl_t_r = matrixl_t.repeat(self.pose_est.size()[0],1,1)
#print(matrix_t.size())
#print(self.obs_global_est.size())
size_local = self.pose_est.size()
ss = torch.zeros(0).cuda()
for i in range(size_local[0]):
tmp = matrixl_t_r[i].mm(self.pose_est[i].unsqueeze(-1)).cuda()
self.pose_est[i] = tmp.squeeze()
#self.obs_global_est = matrix_t * self.obs_global_est
print(self.pose_est.size(),'dsd')
size_global = self.obs_global_est.size()
ss = torch.zeros(0).cuda()
for i in range(size_global[0]):
s = torch.zeros(0).cuda()
for j in range(size_global[1]):
tmp = matrix_t_r[i][j].mm(self.obs_global_est[i][j].unsqueeze(-1)).cuda()
self.obs_global_est[i][j] = tmp.squeeze()
#rint(tmp)
#torch.cat([s,tmp.cuda()],0)
#torch.cat([ss,s],2)
'''
if self.training:
self.unoccupied_local = sample_unoccupied_point(
self.obs_local, self.n_samples)
self.unoccupied_global = transform_to_global_2D(
self.pose_est, self.unoccupied_local)
inputs, self.gt = get_M_net_inputs_labels(self.obs_global_est,
self.unoccupied_global)
self.occp_prob = self.occup_net(inputs)
loss = self.compute_loss()
return loss
def __getitem__(self, index):
pcd = self.point_clouds[index,:,:] # <Lx2>
valid_points = self.valid_points[index,:]
if self._trans_by_pose is not None:
pcd = pcd.unsqueeze(0) # <1XLx2>
pose = self._trans_by_pose[index, :].unsqueeze(0) # <1x3>
pcd = utils.transform_to_global_2D(pose, pcd).squeeze(0)
return pcd,valid_points
def forward(self, obs_local, valid_points):
# obs_local: <BxLx2>
self.obs_local = deepcopy(obs_local)
self.valid_points = valid_points
self.pose_est = self.loc_net(self.obs_local)
self.obs_global_est = transform_to_global_2D(self.pose_est,
self.obs_local)
if self.training:
self.unoccupied_local = sample_unoccupied_point(
self.obs_local, self.n_samples)
self.unoccupied_global = transform_to_global_2D(
self.pose_est, self.unoccupied_local)
inputs, self.gt = get_M_net_inputs_labels(self.obs_global_est,
self.unoccupied_global)
self.occp_prob = self.occup_net(inputs)
loss = self.compute_loss()
return loss
def forward(self, obs_local, valid_points, sensor_pose, masks):
# obs_local: <BxLx2>
# sensor_pose: init pose <Bx1x3>
self.masks = masks
self.obs_local = deepcopy(obs_local)
self.valid_points = valid_points
self.pose_est = self.loc_net(self.obs_local)
self.obs_global_est = transform_to_global_2D(self.pose_est,
self.obs_local)
if self.training:
sensor_center = sensor_pose[:, :, :2]
self.unoccupied_local = sample_unoccupied_point(
self.obs_local, self.n_samples, sensor_center)
self.unoccupied_global = transform_to_global_2D(
self.pose_est, self.unoccupied_local)
inputs, self.gt = get_M_net_inputs_labels(self.obs_global_est,
self.unoccupied_global)
self.occp_prob = self.occup_net(inputs)
return self.compute_loss()
n_pc = len(dataset)
pose_est = np.zeros((n_pc, 3), dtype=np.float32)
print('running icp')
for idx in range(n_pc - 1):
dst, valid_dst = dataset[idx]
src, valid_src = dataset[idx + 1]
dst = dst[valid_dst, :].numpy()
src = src[valid_src, :].numpy()
_, R0, t0 = utils.icp(src, dst, metrics=opt.metric)
if idx == 0:
R_cum = R0
t_cum = t0
else:
R_cum = np.matmul(R_cum, R0)
t_cum = np.matmul(R_cum, t0) + t_cum
pose_est[idx + 1, :2] = t_cum.T
pose_est[idx + 1, 2] = np.arctan2(R_cum[1, 0], R_cum[0, 0])
save_name = os.path.join(checkpoint_dir, 'pose_est.npy')
np.save(save_name, pose_est)
print('saving results')
pose_est = torch.from_numpy(pose_est)
local_pc, valid_id = dataset[:]
global_pc = utils.transform_to_global_2D(pose_est, local_pc)
utils.plot_global_point_cloud(global_pc, pose_est, valid_id, checkpoint_dir)
