|
8 | 8 | # needed to import for allowing type-hinting: torch.device | str | None |
9 | 9 | from __future__ import annotations |
10 | 10 |
|
11 | | -import math |
12 | 11 | import numpy as np |
13 | 12 | import torch |
14 | | -import torch.nn.functional as F |
15 | 13 | from collections.abc import Sequence |
16 | | -from typing import Literal |
17 | 14 |
|
18 | | -import omni.isaac.core.utils.stage as stage_utils |
19 | 15 | import warp as wp |
20 | | -from pxr import UsdGeom |
21 | 16 |
|
22 | 17 | import omni.isaac.lab.utils.math as math_utils |
23 | 18 | from omni.isaac.lab.utils.array import TensorData, convert_to_torch |
@@ -262,143 +257,6 @@ def create_pointcloud_from_rgbd( |
262 | 257 | return points_xyz, points_rgb |
263 | 258 |
|
264 | 259 |
|
265 | | -def convert_orientation_convention( |
266 | | - orientation: torch.Tensor, |
267 | | - origin: Literal["opengl", "ros", "world"] = "opengl", |
268 | | - target: Literal["opengl", "ros", "world"] = "ros", |
269 | | -) -> torch.Tensor: |
270 | | - r"""Converts a quaternion representing a rotation from one convention to another. |
271 | | -
|
272 | | - In USD, the camera follows the ``"opengl"`` convention. Thus, it is always in **Y up** convention. |
273 | | - This means that the camera is looking down the -Z axis with the +Y axis pointing up , and +X axis pointing right. |
274 | | - However, in ROS, the camera is looking down the +Z axis with the +Y axis pointing down, and +X axis pointing right. |
275 | | - Thus, the camera needs to be rotated by :math:`180^{\circ}` around the X axis to follow the ROS convention. |
276 | | -
|
277 | | - .. math:: |
278 | | -
|
279 | | - T_{ROS} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD} |
280 | | -
|
281 | | - On the other hand, the typical world coordinate system is with +X pointing forward, +Y pointing left, |
282 | | - and +Z pointing up. The camera can also be set in this convention by rotating the camera by :math:`90^{\circ}` |
283 | | - around the X axis and :math:`-90^{\circ}` around the Y axis. |
284 | | -
|
285 | | - .. math:: |
286 | | -
|
287 | | - T_{WORLD} = \begin{bmatrix} 0 & 0 & -1 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD} |
288 | | -
|
289 | | - Thus, based on their application, cameras follow different conventions for their orientation. This function |
290 | | - converts a quaternion from one convention to another. |
291 | | -
|
292 | | - Possible conventions are: |
293 | | -
|
294 | | - - :obj:`"opengl"` - forward axis: -Z - up axis +Y - Offset is applied in the OpenGL (Usd.Camera) convention |
295 | | - - :obj:`"ros"` - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention |
296 | | - - :obj:`"world"` - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention |
297 | | -
|
298 | | - Args: |
299 | | - orientation: Quaternion of form `(w, x, y, z)` with shape (..., 4) in source convention |
300 | | - origin: Convention to convert to. Defaults to "ros". |
301 | | - target: Convention to convert from. Defaults to "opengl". |
302 | | -
|
303 | | - Returns: |
304 | | - Quaternion of form `(w, x, y, z)` with shape (..., 4) in target convention |
305 | | - """ |
306 | | - if target == origin: |
307 | | - return orientation.clone() |
308 | | - |
309 | | - # -- unify input type |
310 | | - if origin == "ros": |
311 | | - # convert from ros to opengl convention |
312 | | - rotm = math_utils.matrix_from_quat(orientation) |
313 | | - rotm[:, :, 2] = -rotm[:, :, 2] |
314 | | - rotm[:, :, 1] = -rotm[:, :, 1] |
315 | | - # convert to opengl convention |
316 | | - quat_gl = math_utils.quat_from_matrix(rotm) |
317 | | - elif origin == "world": |
318 | | - # convert from world (x forward and z up) to opengl convention |
319 | | - rotm = math_utils.matrix_from_quat(orientation) |
320 | | - rotm = torch.matmul( |
321 | | - rotm, |
322 | | - math_utils.matrix_from_euler( |
323 | | - torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ" |
324 | | - ), |
325 | | - ) |
326 | | - # convert to isaac-sim convention |
327 | | - quat_gl = math_utils.quat_from_matrix(rotm) |
328 | | - else: |
329 | | - quat_gl = orientation |
330 | | - |
331 | | - # -- convert to target convention |
332 | | - if target == "ros": |
333 | | - # convert from opengl to ros convention |
334 | | - rotm = math_utils.matrix_from_quat(quat_gl) |
335 | | - rotm[:, :, 2] = -rotm[:, :, 2] |
336 | | - rotm[:, :, 1] = -rotm[:, :, 1] |
337 | | - return math_utils.quat_from_matrix(rotm) |
338 | | - elif target == "world": |
339 | | - # convert from opengl to world (x forward and z up) convention |
340 | | - rotm = math_utils.matrix_from_quat(quat_gl) |
341 | | - rotm = torch.matmul( |
342 | | - rotm, |
343 | | - math_utils.matrix_from_euler( |
344 | | - torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ" |
345 | | - ).T, |
346 | | - ) |
347 | | - return math_utils.quat_from_matrix(rotm) |
348 | | - else: |
349 | | - return quat_gl.clone() |
350 | | - |
351 | | - |
352 | | -# @torch.jit.script |
353 | | -def create_rotation_matrix_from_view( |
354 | | - eyes: torch.Tensor, |
355 | | - targets: torch.Tensor, |
356 | | - device: str = "cpu", |
357 | | -) -> torch.Tensor: |
358 | | - """ |
359 | | - This function takes a vector ''eyes'' which specifies the location |
360 | | - of the camera in world coordinates and the vector ''targets'' which |
361 | | - indicate the position of the object. |
362 | | - The output is a rotation matrix representing the transformation |
363 | | - from world coordinates -> view coordinates. |
364 | | -
|
365 | | - The inputs camera_position and targets can each be a |
366 | | - - 3 element tuple/list |
367 | | - - torch tensor of shape (1, 3) |
368 | | - - torch tensor of shape (N, 3) |
369 | | -
|
370 | | - Args: |
371 | | - eyes: position of the camera in world coordinates |
372 | | - targets: position of the object in world coordinates |
373 | | -
|
374 | | - The vectors are broadcast against each other so they all have shape (N, 3). |
375 | | -
|
376 | | - Returns: |
377 | | - R: (N, 3, 3) batched rotation matrices |
378 | | -
|
379 | | - Reference: |
380 | | - Based on PyTorch3D (https://github.com/facebookresearch/pytorch3d/blob/eaf0709d6af0025fe94d1ee7cec454bc3054826a/pytorch3d/renderer/cameras.py#L1635-L1685) |
381 | | - """ |
382 | | - up_axis_token = stage_utils.get_stage_up_axis() |
383 | | - if up_axis_token == UsdGeom.Tokens.y: |
384 | | - up_axis = torch.tensor((0, 1, 0), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1) |
385 | | - elif up_axis_token == UsdGeom.Tokens.z: |
386 | | - up_axis = torch.tensor((0, 0, 1), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1) |
387 | | - else: |
388 | | - raise ValueError(f"Invalid up axis: {up_axis_token}") |
389 | | - |
390 | | - # get rotation matrix in opengl format (-Z forward, +Y up) |
391 | | - z_axis = -F.normalize(targets - eyes, eps=1e-5) |
392 | | - x_axis = F.normalize(torch.cross(up_axis, z_axis, dim=1), eps=1e-5) |
393 | | - y_axis = F.normalize(torch.cross(z_axis, x_axis, dim=1), eps=1e-5) |
394 | | - is_close = torch.isclose(x_axis, torch.tensor(0.0), atol=5e-3).all(dim=1, keepdim=True) |
395 | | - if is_close.any(): |
396 | | - replacement = F.normalize(torch.cross(y_axis, z_axis, dim=1), eps=1e-5) |
397 | | - x_axis = torch.where(is_close, replacement, x_axis) |
398 | | - R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1) |
399 | | - return R.transpose(1, 2) |
400 | | - |
401 | | - |
402 | 260 | def save_images_to_file(images: torch.Tensor, file_path: str): |
403 | 261 | """Save images to file. |
404 | 262 |
|
|
0 commit comments