def get_rgbd_cloud(self, rgb, depth, segmentation, pose2d):
return od.geometry.PointCloud.create_from_rgbd_image(
image=self.camera.convertToO3dRGBD(rgb, depth),
intrinsic=self.camera.intrinsic,
extrinsic=np.linalg.inv(
np.matmul(self.camera.extrinsic,
pose_to_transformation(pose2d))))
def integrate(self, rgb, depth, segmentation, pose2d):
""" Integrate a new depth and rgb image to the current TSDF. """
self._volume.integrateRGBDSeg(
self.camera.convertToO3d(rgb, depth, segmentation),
self.camera.intrinsic,
np.linalg.inv(
np.matmul(self.camera.extrinsic,
pose_to_transformation(pose2d))))
def observe_local(self, pose2d):
"""Request local grid to be extracted"""
origin = np.matmul(pose_to_transformation(pose2d), self._origin_robot)
# Get first 3 elements.
origin = origin[:3]
self.tsdf_grid = self._volume.extract_tsdf(self._voxel_length_m,
self._width_m,
self._height_m,
self._depth_m, origin,
pose2d[2])
def get_cylinder(self, pose2d):
return o3d.geometry.TriangleMesh(self.mesh_cylinder).transform(
pose_to_transformation(pose2d))
def get_frame(self, pose2d):
return o3d.geometry.TriangleMesh(self.mesh_frame).transform(
pose_to_transformation(pose2d))
def get_visuals(self, pose2d):
T = pose_to_transformation(pose2d)
agent = o3d.geometry.TriangleMesh(self.mesh_cylinder)
frame_w = o3d.geometry.TriangleMesh(self.mesh_frame_W)
frame = o3d.geometry.TriangleMesh(self.mesh_frame)
return agent.transform(T), frame_w.transform(T), frame