| 318 | return new_cloud_msg |
| 319 | |
| 320 | def _transform_cloud(self,transform_msg): |
| 321 | # Transform ('map' coordinates --> 'camera_rgb_optical_frame' coordinates) |
| 322 | startTime = time.time() |
| 323 | transform_array = torch.FloatTensor(msg_to_se3(transform_msg)).to(opt.device) # (4,4) |
| 324 | transformed_cloud_xyz = torch.matmul(transform_array,self.raw_cloud_xyz1)[:3] # (3,num_points) |
| 325 | transformed_cloud = torch.cat([transformed_cloud_xyz,self.raw_cloud_rgb],dim=0) |
| 326 | transformed_cloud = transformed_cloud.transpose(1,0) |
| 327 | return transformed_cloud # (num_points,6) |
| 328 | |
| 329 | def _filter_out_of_view_points(self,cloud_array): |
| 330 | #k keep only [20%] percent of points |