diff --git a/source/isaaclab/changelog.d/liorbenhorin-opencv-lens-distortion-cameras-newton.skip b/source/isaaclab/changelog.d/liorbenhorin-opencv-lens-distortion-cameras-newton.skip new file mode 100644 index 000000000000..68fd217dcc63 --- /dev/null +++ b/source/isaaclab/changelog.d/liorbenhorin-opencv-lens-distortion-cameras-newton.skip @@ -0,0 +1 @@ +Documentation-only update describing existing Newton renderer support; no separate Isaac Lab API change. diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index dccf09b2e0b1..34f3e5302622 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -19,9 +19,9 @@ class OpenCvDistortionCfg: """Base configuration for an OpenCV lens-distortion model carried on a camera cfg. The distortion model is renderer-agnostic: it is stored on the camera spawn configuration - and each renderer decides how to consume it. Under the RTX/OVRTX renderer the fields are + and each renderer decides how to consume it. Under the RTX/OVRTX renderer, the fields are authored as the ``omni:lensdistortion:*`` USD API, which the renderer honors natively. The - Newton renderer does not yet apply this model. + Newton renderer consumes the same configuration to generate calibrated, distorted camera rays. The intrinsic parameters (:attr:`fx`, :attr:`fy`, :attr:`cx`, :attr:`cy`) follow the OpenCV convention. When a distortion model is present, they take precedence over the focal-length @@ -151,18 +151,18 @@ class PinholeCameraCfg(SpawnerCfg): Note: The stock projection is ``"pinhole"``. An OpenCV ``fx/fy/cx/cy`` + distortion-coefficient intrinsic model can be applied on top via :attr:`distortion` (see - :class:`OpenCvPinholeDistortionCfg` / :class:`OpenCvFisheyeDistortionCfg`), which the - RTX/OVRTX renderer honors natively. + :class:`OpenCvPinholeDistortionCfg` / :class:`OpenCvFisheyeDistortionCfg`). The RTX/OVRTX + renderer honors this model natively, while the Newton renderer generates calibrated + per-pixel rays from it. """ distortion: OpenCvDistortionCfg | None = None """OpenCV lens-distortion model to author on the camera. Defaults to None (no distortion). - When set, the OpenCV intrinsics and distortion coefficients are authored on the camera prim. - Under the RTX/OVRTX renderer they drive the projection natively and, when a real calibration is - used (``fx != fy`` or an off-center principal point), take precedence over the focal-length and - aperture projection. The Newton renderer does not yet apply this model; the camera renders - undistorted there. + When set, the OpenCV intrinsics and distortion coefficients drive the camera projection. The + RTX/OVRTX renderer consumes the authored ``omni:lensdistortion:*`` USD API, while the Newton + renderer generates calibrated per-pixel rays directly from this configuration. In both cases, + the calibrated intrinsics take precedence over the focal-length and aperture projection. """ clipping_range: tuple[float, float] = (0.01, 1e6) diff --git a/source/isaaclab_newton/changelog.d/opencv-lens-distortion-cameras.rst b/source/isaaclab_newton/changelog.d/opencv-lens-distortion-cameras.rst new file mode 100644 index 000000000000..ef005fa81da2 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/opencv-lens-distortion-cameras.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added OpenCV pinhole and fisheye lens-distortion rendering to the Newton backend, including + calibrated intrinsics and coefficient muting through ``apply_lens_distortion``. diff --git a/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py b/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py index dc1cce2cab30..fde4edc58d6b 100644 --- a/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py +++ b/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py @@ -8,6 +8,7 @@ from __future__ import annotations import logging +import math from dataclasses import dataclass from typing import TYPE_CHECKING, Any, NoReturn @@ -130,7 +131,7 @@ def __init__( # Camera clipping planes [m] from ``spawn.clipping_range`` (``[0]`` near, ``[1]`` far). # Newton's ray tracer has no near-plane parameter, so only the far plane is enforced (through # the sensor's ``max_distance``); ``near_clip`` is captured for consumers but not applied. - spawn = getattr(spec.cfg, "spawn", None) + spawn = spec.cfg.spawn clipping_range = getattr(spawn, "clipping_range", None) self.near_clip: float | None = float(clipping_range[0]) if clipping_range is not None else None self.far_clip: float | None = float(clipping_range[1]) if clipping_range is not None else None @@ -145,6 +146,9 @@ def __init__( else: self.clear_color = 0xFFEEEEEE + # OpenCV lens-distortion model (``spawn.distortion``), consumed by :meth:`_build_distortion_rays` + # to trace distorted per-pixel rays instead of the centered, square-pixel pinhole field. + self._distortion = spawn.distortion if spawn is not None else None # Post-render PPISP pipeline composed when ``spec.cfg.isp_cfg`` is set. # ``isp_cfg`` is already fully normalized by ``prepare_cameras`` by the time it reaches here. self.ppisp_pipeline: PpispPipeline | None = None @@ -356,14 +360,85 @@ def update(self, positions: ProxyArray, orientations: ProxyArray, intrinsics: Pr ) if self.camera_rays is None: - first_focal_length = intrinsics.torch[:, 1, 1][0:1] - fov_radians_all = 2.0 * torch.atan(self.height / (2.0 * first_focal_length)) + if self._distortion is not None: + self.camera_rays = self._build_distortion_rays() + else: + first_focal_length = intrinsics.torch[:, 1, 1][0:1] + fov_radians_all = 2.0 * torch.atan(self.height / (2.0 * first_focal_length)) + + fov_warp = wp.from_torch(fov_radians_all, dtype=wp.float32) + self.camera_rays = self.newton_sensor.utils.compute_camera_rays_pinhole( + self.width, self.height, camera_fovs=fov_warp + ) - fov_warp = wp.from_torch(fov_radians_all, dtype=wp.float32) - self.camera_rays = self.newton_sensor.utils.compute_camera_rays_pinhole( - self.width, self.height, camera_fovs=fov_warp + def _build_distortion_rays(self) -> wp.array(dtype=wp.vec3f, ndim=4): + """Build the ``(1, H, W, 2)`` camera-space ray field for an OpenCV lens-distortion camera. + + Uses Newton's native OpenCV fisheye ray helper and the Isaac Lab OpenCV pinhole kernel. Both + paths honor calibrated ``fx/fy/cx/cy`` (non-square, off-center) intrinsics. When + :attr:`OpenCvDistortionCfg.apply_lens_distortion` is ``False``, the coefficients are treated + as zero while the calibrated intrinsics remain active, matching the RTX/OVRTX behavior. + """ + from .opencv_distortion_rays import compute_camera_rays_opencv_pinhole + + cfg = self._distortion + device = self.newton_sensor.model.device + image_width, image_height = float(cfg.image_size[0]), float(cfg.image_size[1]) + # ``apply_lens_distortion=False`` keeps the intrinsics but mutes the distortion coefficients. + apply = bool(getattr(cfg, "apply_lens_distortion", True)) + + def _coeff(name: str) -> float: + return float(getattr(cfg, name, 0.0)) if apply else 0.0 + + if cfg.model == "opencvFisheye": + return self.newton_sensor.utils.compute_camera_rays_fisheye_opencv( + self.width, + self.height, + float(cfg.fx), + float(cfg.fy), + float(cfg.cx), + float(cfg.cy), + image_width=image_width, + image_height=image_height, + k1=_coeff("k1"), + k2=_coeff("k2"), + k3=_coeff("k3"), + k4=_coeff("k4"), + # Limit fisheye rays to the forward hemisphere. + max_fov=math.pi, ) + rays = wp.empty((1, self.height, self.width, 2), dtype=wp.vec3f, device=device) + wp.launch( + compute_camera_rays_opencv_pinhole, + dim=(1, self.height, self.width), + inputs=[ + self.width, + self.height, + float(cfg.fx), + float(cfg.fy), + float(cfg.cx), + float(cfg.cy), + image_width, + image_height, + _coeff("k1"), + _coeff("k2"), + _coeff("k3"), + _coeff("k4"), + _coeff("k5"), + _coeff("k6"), + _coeff("p1"), + _coeff("p2"), + _coeff("s1"), + _coeff("s2"), + _coeff("s3"), + _coeff("s4"), + ], + outputs=[rays], + device=device, + ) + return rays + @wp.kernel def _update_transforms( positions: wp.array(dtype=wp.vec3f), @@ -473,20 +548,9 @@ def prepare_cameras(self, stage: Any, spec: CameraRenderSpec) -> None: Also captures the USD ``stage`` so the segmentation mapper can read the scene's :class:`UsdSemantics.LabelsAPI` labels when a segmentation output is requested. + """ self._stage = stage - # NOTE: OpenCV lens distortion (``spawn.distortion``) is not yet applied by the Newton - # renderer. The distortion cfg is renderer-agnostic and could be piped through Newton's warp - # ray-tracing utilities here in the future; for now the camera renders undistorted. This is - # the intended extension point. - spawn = getattr(spec.cfg, "spawn", None) - if getattr(spawn, "distortion", None) is not None: - logger.warning( - "OpenCV lens distortion is set on the camera cfg but is not yet applied by the Newton" - " renderer: it derives a single field of view from fy, so the distortion coefficients," - " the principal point, and a non-square fx are ignored and the camera renders as a" - " centered, square-pixel pinhole. Use the RTX/OVRTX renderer to apply the full model." - ) if spec.cfg.isp_cfg is None: return try: diff --git a/source/isaaclab_newton/isaaclab_newton/renderers/opencv_distortion_rays.py b/source/isaaclab_newton/isaaclab_newton/renderers/opencv_distortion_rays.py new file mode 100644 index 000000000000..85330c62da50 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/renderers/opencv_distortion_rays.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Warp ray generation for OpenCV pinhole lens distortion under the Newton renderer.""" + +from __future__ import annotations + +import warp as wp + +# Number of fixed-point iterations used to invert the distortion model. +_INVERSION_ITERATIONS = 20 + + +@wp.kernel(enable_backward=False) +def compute_camera_rays_opencv_pinhole( + width: int, + height: int, + fx: wp.float32, + fy: wp.float32, + cx: wp.float32, + cy: wp.float32, + image_width: wp.float32, + image_height: wp.float32, + k1: wp.float32, + k2: wp.float32, + k3: wp.float32, + k4: wp.float32, + k5: wp.float32, + k6: wp.float32, + p1: wp.float32, + p2: wp.float32, + s1: wp.float32, + s2: wp.float32, + s3: wp.float32, + s4: wp.float32, + out_rays: wp.array(dtype=wp.vec3f, ndim=4), +): + """Emit camera-space rays for an OpenCV pinhole (rational + tangential + thin-prism) camera. + + The forward OpenCV model maps an undistorted normalized point ``(x, y)`` (with ``r2 = x^2 + y^2``) + to the distorted normalized point ``(x_d, y_d)`` via a rational radial term, tangential terms + (``p1``, ``p2``) and thin-prism terms (``s1..s4``). For each output pixel the distorted point is + known from the pixel coordinate and the intrinsics; the kernel recovers the undistorted point by + fixed-point iteration (matching OpenCV's :func:`undistortPoints`) and forms the ray. + + Args: + width: Output image width [px]. + height: Output image height [px]. + fx: Focal length along the image x-axis [px]. + fy: Focal length along the image y-axis [px]. + cx: Principal point x-coordinate [px]. + cy: Principal point y-coordinate [px]. + image_width: Calibrated image width the intrinsics refer to [px]. + image_height: Calibrated image height the intrinsics refer to [px]. + k1: First radial distortion coefficient (numerator). + k2: Second radial distortion coefficient (numerator). + k3: Third radial distortion coefficient (numerator). + k4: First radial distortion coefficient (denominator, rational model). + k5: Second radial distortion coefficient (denominator, rational model). + k6: Third radial distortion coefficient (denominator, rational model). + p1: First tangential distortion coefficient. + p2: Second tangential distortion coefficient. + s1: First thin-prism distortion coefficient. + s2: Second thin-prism distortion coefficient. + s3: Third thin-prism distortion coefficient. + s4: Fourth thin-prism distortion coefficient. + out_rays: Ray field of shape ``(1, height, width, 2)``: ``[..., 0]`` origin, ``[..., 1]`` + direction, both in Newton's OpenGL camera space. + """ + camera_index, py, px = wp.tid() + + # Map the render pixel onto the calibrated image grid, then to distorted normalized coordinates. + # Match Newton's OpenCV fisheye pixel-grid sampling convention. + # OpenCV image y points down. + u = ((wp.float32(px) + 0.5) / wp.float32(width)) * image_width + v = ((wp.float32(py) + 0.5) / wp.float32(height)) * image_height + x_d = (u - cx) / fx + y_d = (v - cy) / fy + + # Fixed-point inversion of the forward model, seeded at the distorted point. + x = x_d + y = y_d + for _i in range(_INVERSION_ITERATIONS): + r2 = x * x + y * y + r4 = r2 * r2 + r6 = r4 * r2 + radial = (1.0 + k1 * r2 + k2 * r4 + k3 * r6) / (1.0 + k4 * r2 + k5 * r4 + k6 * r6) + dx = 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x) + s1 * r2 + s2 * r4 + dy = p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y + s3 * r2 + s4 * r4 + x = (x_d - dx) / radial + y = (y_d - dy) / radial + + # OpenCV camera space (+Z forward, +Y down) -> Newton OpenGL camera space (-Z forward, +Y up). + ray_direction_camera_space = wp.normalize(wp.vec3f(x, -y, -1.0)) + out_rays[camera_index, py, px, 0] = wp.vec3f(0.0) + out_rays[camera_index, py, px, 1] = ray_direction_camera_space diff --git a/source/isaaclab_newton/test/renderers/__init__.py b/source/isaaclab_newton/test/renderers/__init__.py new file mode 100644 index 000000000000..9c04cb0f85c2 --- /dev/null +++ b/source/isaaclab_newton/test/renderers/__init__.py @@ -0,0 +1,5 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +"""Tests for Newton renderers.""" diff --git a/source/isaaclab_newton/test/renderers/test_opencv_distortion_rays.py b/source/isaaclab_newton/test/renderers/test_opencv_distortion_rays.py new file mode 100644 index 000000000000..ebda79afb43d --- /dev/null +++ b/source/isaaclab_newton/test/renderers/test_opencv_distortion_rays.py @@ -0,0 +1,163 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Kit-less, GPU-less tests for the OpenCV pinhole distortion ray-generation kernel. + +These exercise the Warp kernel in +:mod:`isaaclab_newton.renderers.opencv_distortion_rays` on the warp CPU device, without ``newton``, +a renderer or a GPU. The kernel inverts the OpenCV forward distortion model to recover the +camera-space ray for each output pixel. Correctness is checked by re-applying the OpenCV *forward* +model (computed here in NumPy) to the recovered undistorted point and confirming it lands back on the +originating pixel (round-trip), which is the property the fixed-point inversion must satisfy. + +OpenCV fisheye rays use Newton's native ``compute_camera_rays_fisheye_opencv`` helper and are covered +by the Newton camera integration test. +""" + +from __future__ import annotations + +import importlib.util + +import numpy as np +import pytest + +_REQUIRED_MODULES = ("warp",) +_MISSING_MODULES = [module for module in _REQUIRED_MODULES if importlib.util.find_spec(module) is None] + +pytestmark = [ + pytest.mark.unit, + pytest.mark.skipif( + bool(_MISSING_MODULES), + reason=f"requires optional modules: {', '.join(_MISSING_MODULES)}", + ), +] + +if not _MISSING_MODULES: + import warp as wp + from isaaclab_newton.renderers.opencv_distortion_rays import compute_camera_rays_opencv_pinhole + +WIDTH, HEIGHT = 64, 48 +_CALIB = dict(fx=339.26592887, fy=338.82010626, cx=323.55809091, cy=250.27360914) +# calibrated image the intrinsics refer to (the render grid is smaller and rescaled onto it) +_IMAGE_W, _IMAGE_H = 640, 480 +_PINHOLE_COEFFS = dict( + k1=0.1, + k2=-0.05, + k3=0.01, + k4=0.005, + k5=-0.002, + k6=0.0005, + p1=0.001, + p2=-0.002, + s1=0.0005, + s2=-0.0002, + s3=0.0003, + s4=-0.0001, +) + + +def _launch_pinhole(coeffs: dict) -> np.ndarray: + """Launch the pinhole kernel on the warp CPU device and return the ray field as NumPy.""" + rays = wp.empty((1, HEIGHT, WIDTH, 2), dtype=wp.vec3f, device="cpu") + wp.launch( + compute_camera_rays_opencv_pinhole, + dim=(1, HEIGHT, WIDTH), + inputs=[ + WIDTH, + HEIGHT, + _CALIB["fx"], + _CALIB["fy"], + _CALIB["cx"], + _CALIB["cy"], + float(_IMAGE_W), + float(_IMAGE_H), + coeffs["k1"], + coeffs["k2"], + coeffs["k3"], + coeffs["k4"], + coeffs["k5"], + coeffs["k6"], + coeffs["p1"], + coeffs["p2"], + coeffs["s1"], + coeffs["s2"], + coeffs["s3"], + coeffs["s4"], + ], + outputs=[rays], + device="cpu", + ) + return rays.numpy() + + +def _pixel_distorted_normalized(px: int, py: int) -> tuple[float, float]: + """The OpenCV *distorted* normalized coordinates the kernel derives from a render pixel.""" + u = ((px + 0.5) / WIDTH) * _IMAGE_W + v = ((py + 0.5) / HEIGHT) * _IMAGE_H + x_d = (u - _CALIB["cx"]) / _CALIB["fx"] + y_d = (v - _CALIB["cy"]) / _CALIB["fy"] + return x_d, y_d + + +def _forward_pinhole(x: float, y: float, c: dict) -> tuple[float, float]: + """OpenCV pinhole forward model: undistorted normalized ``(x, y)`` -> distorted normalized.""" + r2 = x * x + y * y + r4, r6 = r2 * r2, r2 * r2 * r2 + radial = (1.0 + c["k1"] * r2 + c["k2"] * r4 + c["k3"] * r6) / (1.0 + c["k4"] * r2 + c["k5"] * r4 + c["k6"] * r6) + x_d = x * radial + 2.0 * c["p1"] * x * y + c["p2"] * (r2 + 2.0 * x * x) + c["s1"] * r2 + c["s2"] * r4 + y_d = y * radial + c["p1"] * (r2 + 2.0 * y * y) + 2.0 * c["p2"] * x * y + c["s3"] * r2 + c["s4"] * r4 + return x_d, y_d + + +def _ray_to_opencv_normalized(direction: np.ndarray) -> tuple[float, float]: + """Map a Newton OpenGL camera-space ray back to OpenCV undistorted normalized ``(x_u, y_u)``. + + The kernel emits ``normalize(vec3(x_u, -y_u, -1))``; undo the normalization and the ``y``/``z`` + sign flip that maps OpenCV camera space onto Newton's OpenGL camera space. + """ + dx, dy, dz = float(direction[0]), float(direction[1]), float(direction[2]) + # dz corresponds to -1 before normalization, so scale by -1/dz to recover the z == 1 plane. + x_u = dx / (-dz) + y_u = -dy / (-dz) + return x_u, y_u + + +def test_pinhole_ray_origins_are_zero_and_directions_unit(): + """Every ray has a zero origin and a unit-length direction.""" + rays = _launch_pinhole(_PINHOLE_COEFFS) + origins = rays[..., 0, :] + directions = rays[..., 1, :] + assert np.allclose(origins, 0.0) + norms = np.linalg.norm(directions, axis=-1) + assert np.allclose(norms, 1.0, atol=1e-5) + # all rays look down -Z in Newton's OpenGL camera space + assert np.all(directions[..., 2] < 0.0) + + +def test_pinhole_inversion_round_trips_to_pixel(): + """Re-applying the OpenCV forward model to the recovered ray lands back on each pixel's distorted point.""" + rays = _launch_pinhole(_PINHOLE_COEFFS) + directions = rays[0, :, :, 1, :] + max_err = 0.0 + for py in range(0, HEIGHT, 7): + for px in range(0, WIDTH, 9): + x_u, y_u = _ray_to_opencv_normalized(directions[py, px]) + x_d_fwd, y_d_fwd = _forward_pinhole(x_u, y_u, _PINHOLE_COEFFS) + x_d, y_d = _pixel_distorted_normalized(px, py) + max_err = max(max_err, abs(x_d_fwd - x_d), abs(y_d_fwd - y_d)) + assert max_err < 1e-5, f"pinhole inversion round-trip error {max_err:.2e} too large" + + +def test_pinhole_zero_coeffs_matches_ideal_projection(): + """With zero coefficients the recovered ray is the plain pinhole ray through the pixel.""" + zero = {k: 0.0 for k in _PINHOLE_COEFFS} + rays = _launch_pinhole(zero) + directions = rays[0, :, :, 1, :] + for py in range(0, HEIGHT, 11): + for px in range(0, WIDTH, 13): + x_u, y_u = _ray_to_opencv_normalized(directions[py, px]) + x_d, y_d = _pixel_distorted_normalized(px, py) + assert x_u == pytest.approx(x_d, abs=1e-5) + assert y_u == pytest.approx(y_d, abs=1e-5) diff --git a/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py b/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py new file mode 100644 index 000000000000..b54c82c35bfb --- /dev/null +++ b/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py @@ -0,0 +1,257 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Validate that the Newton warp renderer applies the OpenCV lens-distortion camera model. + +A ground plane is rendered through a camera carrying an OpenCV pinhole (or fisheye) calibration. The +Newton renderer inverts the OpenCV forward model per output pixel to trace the distorted camera-space +rays, so a per-pixel ray-hit distance (``distance_to_camera``) map warps under the distortion. The +distance map is used as the comparison signal because it is purely geometric: it does not depend on +scene textures (which Newton skips without Kit), so the distortion effect is visible across the whole +frame rather than only on sparse textured features. + +With the coefficients applied vs. muted (``apply_lens_distortion=False``) the same calibrated camera +produces meaningfully different distance maps; the OpenCV fisheye projection likewise differs from an +undistorted pinhole. + +Notes: + * Runs against the Newton warp renderer (no Kit/Isaac Sim, no OVRTX). It requires ``newton`` and a + CUDA GPU; it skips cleanly otherwise. + * Uses Newton physics (``NewtonCfg`` + ``MJWarpSolverCfg``) so the scene is built through the + Newton model the warp renderer traces. +""" + +from __future__ import annotations + +import importlib.util + +import numpy as np +import pytest + +pytestmark = [pytest.mark.integration, pytest.mark.rendering] + +_REQUIRED_MODULES = ("isaaclab_newton", "newton", "warp", "torch") +_MISSING_MODULES = [module for module in _REQUIRED_MODULES if importlib.util.find_spec(module) is None] + + +def _cuda_available() -> bool: + """Whether a CUDA device is available for the Newton warp renderer.""" + if _MISSING_MODULES: + return False + import torch + + return torch.cuda.is_available() + + +_SKIP_NO_NEWTON = pytest.mark.skipif( + bool(_MISSING_MODULES), + reason=f"requires optional modules: {', '.join(_MISSING_MODULES)}", +) +_SKIP_NO_CUDA = pytest.mark.skipif( + not _cuda_available(), + reason="requires a CUDA GPU for the Newton warp renderer", +) + +if not _MISSING_MODULES: + import torch + from isaaclab_newton.physics.mjwarp_manager_cfg import MJWarpSolverCfg + from isaaclab_newton.physics.newton_manager_cfg import NewtonCfg + from isaaclab_newton.renderers import NewtonWarpRendererCfg + + import isaaclab.sim as sim_utils + from isaaclab.assets import AssetBaseCfg, RigidObjectCfg + from isaaclab.scene import InteractiveScene, InteractiveSceneCfg + from isaaclab.sensors import Camera, CameraCfg + from isaaclab.sim import SimulationCfg + from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg + from isaaclab.sim.spawners.sensors.sensors_cfg import ( + OpenCvDistortionCfg, + OpenCvFisheyeDistortionCfg, + OpenCvPinholeDistortionCfg, + PinholeCameraCfg, + ) + from isaaclab.terrains import TerrainImporterCfg + from isaaclab.utils.configclass import configclass + from isaaclab.utils.math import create_rotation_matrix_from_view, quat_from_matrix + +SIM_DT = 1.0 / 60.0 +WIDTH, HEIGHT = 640, 480 +WARMUP_STEPS = 4 + +# OpenCV calibration with non-square focal lengths and an off-center principal point. +_CALIB = dict(fx=339.26592887, fy=338.82010626, cx=323.55809091, cy=250.27360914) +# The radial map r_d = r_u * (1 + k1 * r_u**2) is globally monotonic because +# its derivative is 1 + 3 * k1 * r_u**2 > 0. +_PINHOLE_K1 = 0.1 +# OpenCV fisheye (equidistant) coefficients; the base fisheye projection alone differs strongly from pinhole +_FISHEYE_COEFFS = dict(k1=0.1, k2=-0.05, k3=0.0, k4=0.0) + +_CAM_EYE = (0.0, 0.0, 2.5) +_CAM_TARGET = (1.75, 0.0, 0.0) + + +if not _MISSING_MODULES: + + @configclass + class _DistortionSceneCfg(InteractiveSceneCfg): + """A ground plane, calibrated camera, and off-screen anchor body for Newton.""" + + ground = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane") + dome_light = AssetBaseCfg( + prim_path="/World/DomeLight", + spawn=sim_utils.DomeLightCfg(intensity=2000.0, color=(0.9, 0.9, 0.9)), + ) + anchor = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Anchor", + spawn=sim_utils.CuboidCfg( + size=(0.01, 0.01, 0.01), + rigid_props=sim_utils.RigidBodyBaseCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.001), + collision_props=sim_utils.CollisionBaseCfg(), + physics_material=RigidBodyMaterialBaseCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, -100.0)), + ) + camera = CameraCfg( + prim_path="{ENV_REGEX_NS}/Camera", + update_period=0.0, + height=HEIGHT, + width=WIDTH, + data_types=["distance_to_camera"], + spawn=PinholeCameraCfg(focal_length=13.6, clipping_range=(0.001, 20.0)), + renderer_cfg=NewtonWarpRendererCfg(), + ) + + +def _pinhole_distortion(apply_lens_distortion: bool) -> OpenCvPinholeDistortionCfg: + """Pinhole OpenCV calibration with a globally invertible synthetic radial coefficient.""" + return OpenCvPinholeDistortionCfg( + image_size=(WIDTH, HEIGHT), + apply_lens_distortion=apply_lens_distortion, + k1=_PINHOLE_K1, + **_CALIB, + ) + + +def _fisheye_distortion(apply_lens_distortion: bool) -> OpenCvFisheyeDistortionCfg: + """Fisheye OpenCV calibration reusing the SO-101 intrinsics with fisheye coefficients.""" + return OpenCvFisheyeDistortionCfg( + image_size=(WIDTH, HEIGHT), + apply_lens_distortion=apply_lens_distortion, + **_CALIB, + **_FISHEYE_COEFFS, + ) + + +def _expected_pinhole_ground_distance(px: int, py: int) -> float: + """Compute the expected distorted-ray distance to the ground plane [m].""" + u = px + 0.5 + v = py + 0.5 + x_d = (u - _CALIB["cx"]) / _CALIB["fx"] + y_d = (v - _CALIB["cy"]) / _CALIB["fy"] + radius_d = float(np.hypot(x_d, y_d)) + + if radius_d > 0.0: + lower, upper = 0.0, radius_d + for _ in range(64): + radius_u = 0.5 * (lower + upper) + if radius_u * (1.0 + _PINHOLE_K1 * radius_u**2) < radius_d: + lower = radius_u + else: + upper = radius_u + scale = (0.5 * (lower + upper)) / radius_d + x_u, y_u = x_d * scale, y_d * scale + else: + x_u, y_u = 0.0, 0.0 + + ray_camera = np.array((x_u, -y_u, -1.0)) + ray_camera /= np.linalg.norm(ray_camera) + + eye = np.asarray(_CAM_EYE) + forward = np.asarray(_CAM_TARGET) - eye + z_axis = -forward / np.linalg.norm(forward) + x_axis = np.cross(np.array((0.0, 0.0, 1.0)), z_axis) + x_axis /= np.linalg.norm(x_axis) + y_axis = np.cross(z_axis, x_axis) + ray_world = np.column_stack((x_axis, y_axis, z_axis)) @ ray_camera + return float(-eye[2] / ray_world[2]) + + +def _render_distance(distortion: OpenCvDistortionCfg, device: str) -> np.ndarray: + """Render the ground-plane distance map through an OpenCV-calibrated Newton camera. + + ``distance_to_camera`` (per-pixel ray-hit distance [m]) is used instead of ``rgb`` because it is + purely geometric and does not depend on scene textures, which Newton skips without Kit. + """ + sim_utils.create_new_stage() + sim = sim_utils.SimulationContext( + SimulationCfg(dt=SIM_DT, physics=NewtonCfg(solver_cfg=MJWarpSolverCfg(), num_substeps=1), device=device) + ) + rot = tuple( + quat_from_matrix( + create_rotation_matrix_from_view(torch.tensor([_CAM_EYE]), torch.tensor([_CAM_TARGET]), up_axis="Z") + )[0].tolist() + ) + scene_cfg = _DistortionSceneCfg(num_envs=1, env_spacing=20.0) + scene_cfg.camera.offset = CameraCfg.OffsetCfg(pos=_CAM_EYE, rot=rot, convention="opengl") + scene_cfg.camera.spawn.distortion = distortion + scene = InteractiveScene(scene_cfg) + camera: Camera = scene["camera"] + try: + sim.reset() + for _ in range(WARMUP_STEPS): + sim.step() + camera.update(SIM_DT, force_recompute=True) + distance = camera.data.output["distance_to_camera"].torch[0].detach().cpu().float().numpy().copy() + return distance + finally: + del camera + del scene + sim.stop() + sim.clear_instance() + + +def _mean_abs_distance_diff(a: np.ndarray, b: np.ndarray) -> float: + """Mean absolute per-pixel distance difference [m] over pixels that hit geometry in both maps.""" + valid = np.isfinite(a) & np.isfinite(b) & (a > 0.0) & (b > 0.0) + assert valid.mean() > 0.5, "too few valid distance samples to compare" + return float(np.abs(a[valid] - b[valid]).mean()) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@_SKIP_NO_NEWTON +@_SKIP_NO_CUDA +def test_opencv_distortion_changes_newton_render(device): + """The Newton renderer must render the distorted and zero-coefficient cameras meaningfully differently.""" + distorted = _render_distance(_pinhole_distortion(True), device=device) + reference = _render_distance(_pinhole_distortion(False), device=device) + + assert distorted.shape == (HEIGHT, WIDTH, 1) + assert np.isfinite(distorted).mean() > 0.9 + assert np.isfinite(reference).mean() > 0.9 + mean_abs_diff = _mean_abs_distance_diff(distorted, reference) + assert mean_abs_diff > 0.01, f"distorted vs reference distance maps differ by only {mean_abs_diff:.4f} m" + for px, py in ((0, 0), (WIDTH // 2, HEIGHT // 2), (WIDTH - 1, HEIGHT - 1)): + assert distorted[py, px, 0] == pytest.approx(_expected_pinhole_ground_distance(px, py), abs=2e-3) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@_SKIP_NO_NEWTON +@_SKIP_NO_CUDA +def test_opencv_fisheye_distortion_renders_through_newton(device): + """The Newton renderer honors the OpenCV fisheye model: its render differs meaningfully from the pinhole. + + The same calibrated camera is rendered under the OpenCV fisheye model and under an undistorted + pinhole. The fisheye equidistant projection bends the rays, so the two distance maps must differ + well beyond render noise. + """ + fisheye = _render_distance(_fisheye_distortion(True), device=device) + pinhole = _render_distance(_pinhole_distortion(False), device=device) + + assert fisheye.shape == (HEIGHT, WIDTH, 1) + assert np.isfinite(fisheye).mean() > 0.9 + assert np.isfinite(pinhole).mean() > 0.9 + mean_abs_diff = _mean_abs_distance_diff(fisheye, pinhole) + assert mean_abs_diff > 0.05, f"fisheye vs pinhole distance maps differ by only {mean_abs_diff:.4f} m"