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)