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..cb8fc552b617 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/opencv-lens-distortion-cameras.rst @@ -0,0 +1,11 @@ +Added +^^^^^ + +* Added OpenCV lens-distortion support to the Newton renderer: a camera cfg carrying an OpenCV + pinhole (``k1..k6``, ``p1``, ``p2``, ``s1..s4``) or fisheye (``k1..k4``) distortion model on + ``spawn.distortion`` is now rendered through the distortion instead of as a centered, square-pixel + pinhole. Fisheye cameras use Newton's native OpenCV fisheye ray helper, while pinhole cameras use + an Isaac Lab kernel supporting rational radial, tangential, and thin-prism distortion. Both paths + honor the calibrated ``fx/fy/cx/cy`` intrinsics (including non-square focal lengths and an + off-center principal point). With ``apply_lens_distortion=False`` the distortion coefficients are + muted while the intrinsics are still applied, matching the RTX/OVRTX behavior. 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..8b71a4202d01 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 @@ -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 = getattr(spawn, "distortion", 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,86 @@ 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"), + # Match the PR kernel's forward-facing camera hemisphere and avoid validating the + # OpenCV polynomial outside its physically meaningful calibration range. + 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 +549,12 @@ 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. + + OpenCV lens distortion (``spawn.distortion``) needs no preparation here: it is consumed at + ray-generation time by :meth:`RenderData._build_distortion_rays`, which inverts the OpenCV + forward model per pixel to trace the distorted camera-space rays. """ 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..ffb5f20351f5 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/renderers/opencv_distortion_rays.py @@ -0,0 +1,114 @@ +# 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. + +The Newton tiled camera renders by tracing an explicit per-pixel ray field of shape +``(camera_count, height, width, 2)`` (``wp.vec3f``): index ``0`` holds the ray origin in camera +space (always ``wp.vec3f(0.0)``) and index ``1`` the normalized ray direction in camera space. +Newton uses the OpenGL camera convention (``+X`` right, ``+Y`` up, looking down ``-Z``). + +To honor an OpenCV ``fx/fy/cx/cy`` + distortion-coefficient calibration, for each output pixel the +kernel below inverts the OpenCV forward distortion model to recover the *undistorted* normalized +image coordinates ``(x_u, y_u)`` (OpenCV image ``y`` points down), then emit the camera-space ray +``normalize(vec3(x_u, -y_u, -1))`` -- the negation of ``y`` and ``z`` maps OpenCV camera space +(``+Z`` forward, ``+Y`` down) onto Newton's OpenGL camera space. + +OpenCV fisheye ray generation is provided directly by Newton's +``SensorTiledCamera.utils.compute_camera_rays_fisheye_opencv`` helper. +""" + +from __future__ import annotations + +import warp as wp + +# Number of fixed-point iterations used to invert the distortion model. OpenCV's own +# ``undistortPoints`` defaults to a similar iteration budget; the inversion converges well within +# this for realistic calibrations. +_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. + # 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..f0c9244783df --- /dev/null +++ b/source/isaaclab_newton/test/renderers/test_opencv_distortion_rays.py @@ -0,0 +1,165 @@ +# 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.0, k5=0.0, k6=0.0, p1=0.001, p2=-0.002, s1=0.0, s2=0.0, s3=0.0, s4=0.0 +) + + +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) + + +def test_pinhole_off_center_principal_point_is_honored(): + """The ray through the principal-point pixel looks straight ahead (down -Z).""" + rays = _launch_pinhole({k: 0.0 for k in _PINHOLE_COEFFS}) + directions = rays[0, :, :, 1, :] + # pixel closest to the principal point on the render grid + px = int(round(_CALIB["cx"] / _IMAGE_W * WIDTH - 0.5)) + py = int(round(_CALIB["cy"] / _IMAGE_H * HEIGHT - 0.5)) + direction = directions[py, px] + assert direction[0] == pytest.approx(0.0, abs=2e-2) + assert direction[1] == pytest.approx(0.0, abs=2e-2) + assert direction[2] == pytest.approx(-1.0, abs=1e-2) 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..c1be65d97a0d --- /dev/null +++ b/source/isaaclab_newton/test/sensors/test_camera_opencv_distortion_newton.py @@ -0,0 +1,247 @@ +# 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. The reconstructed ``intrinsic_matrices`` are also checked end-to-end against the +authored, non-square, off-center calibration. + +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.sensors.sensors_cfg import ( + OpenCvDistortionCfg, + OpenCvFisheyeDistortionCfg, + OpenCvPinholeDistortionCfg, + PinholeCameraCfg, + ) + 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 + +# Example real-world OpenCV pinhole calibration (fx != fy, off-center principal point). +_CALIB = dict(fx=339.26592887, fy=338.82010626, cx=323.55809091, cy=250.27360914) +_COEFFS = dict(k1=0.07702322, k2=-0.13605453, k3=0.05163219, p1=-0.00024938, p2=-0.00175006) +# scale the (mild) real coefficients so the barrel effect is unambiguous in the assertion +_K_SCALE = 15.0 +# 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): + """The grid-textured ground plane, a dome light and an off-screen anchor body for Newton.""" + + ground = AssetBaseCfg(prim_path="/World/ground", spawn=sim_utils.GroundPlaneCfg()) + 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.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=0.001), + collision_props=sim_utils.CollisionPropertiesCfg(), + physics_material=sim_utils.RigidBodyMaterialCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 0.0)), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, -100.0)), + ) + + +def _pinhole_distortion(apply_lens_distortion: bool) -> OpenCvPinholeDistortionCfg: + """Pinhole OpenCV calibration with the (scaled) SO-101 radial/tangential coefficients.""" + return OpenCvPinholeDistortionCfg( + image_size=(WIDTH, HEIGHT), + apply_lens_distortion=apply_lens_distortion, + **_CALIB, + **{name: value * _K_SCALE for name, value in _COEFFS.items()}, + ) + + +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 _render_distance(distortion: OpenCvDistortionCfg, device: str) -> tuple[np.ndarray, np.ndarray]: + """Render the ground-plane distance map through an OpenCV-calibrated Newton camera; return ``(dist, K)``. + + ``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) + ) + scene = InteractiveScene(_DistortionSceneCfg(num_envs=1, env_spacing=20.0)) + + rot = tuple( + quat_from_matrix( + create_rotation_matrix_from_view(torch.tensor([_CAM_EYE]), torch.tensor([_CAM_TARGET]), up_axis="Z") + )[0].tolist() + ) + camera = Camera( + CameraCfg( + prim_path="/World/envs/env_.*/Camera", + update_period=0.0, + height=HEIGHT, + width=WIDTH, + data_types=["distance_to_camera"], + offset=CameraCfg.OffsetCfg(pos=_CAM_EYE, rot=rot, convention="opengl"), + spawn=PinholeCameraCfg(focal_length=13.6, clipping_range=(0.001, 20.0), distortion=distortion), + renderer_cfg=NewtonWarpRendererCfg(), + ) + ) + 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() + intrinsics = camera.data.intrinsic_matrices.torch[0].detach().cpu().numpy() + return distance, intrinsics + 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) + # both frames render geometry (the ground plane fills the frame) + assert np.isfinite(distorted).mean() > 0.9 + assert np.isfinite(reference).mean() > 0.9 + # the renderer applied the lens distortion: the distance maps warp well beyond render noise + mean_abs_diff = _mean_abs_distance_diff(distorted, reference) + assert mean_abs_diff > 0.05, f"distorted vs reference distance maps differ by only {mean_abs_diff:.4f} m" + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@_SKIP_NO_NEWTON +@_SKIP_NO_CUDA +def test_opencv_distortion_intrinsics_match_authored_newton(device): + """The Newton camera reports intrinsics matching the authored, non-square, off-center calibration.""" + _distance, k = _render_distance(_pinhole_distortion(True), device=device) + + assert k[0, 0] == pytest.approx(_CALIB["fx"], abs=1e-2) + assert k[1, 1] == pytest.approx(_CALIB["fy"], abs=1e-2) + assert k[0, 2] == pytest.approx(_CALIB["cx"], abs=1e-2) + assert k[1, 2] == pytest.approx(_CALIB["cy"], abs=1e-2) + # not the stock fx == fy / centered-principal-point collapse + assert k[0, 0] != k[1, 1] + assert k[0, 2] != pytest.approx(WIDTH / 2) + + +@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, and the reported intrinsics must still match the authored calibration. + """ + fisheye, k = _render_distance(_fisheye_distortion(True), device=device) + pinhole, _ = _render_distance(_pinhole_distortion(False), device=device) + + assert fisheye.shape == (HEIGHT, WIDTH, 1) + # both frames render geometry (the ground plane fills the frame) + assert np.isfinite(fisheye).mean() > 0.9 + assert np.isfinite(pinhole).mean() > 0.9 + # the renderer applied the fisheye projection: the distance map differs from the pinhole beyond noise + 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" + # the fisheye camera still reports the authored, non-square, off-center calibration + assert k[0, 0] == pytest.approx(_CALIB["fx"], abs=1e-2) + assert k[1, 1] == pytest.approx(_CALIB["fy"], abs=1e-2) + assert k[0, 2] == pytest.approx(_CALIB["cx"], abs=1e-2) + assert k[1, 2] == pytest.approx(_CALIB["cy"], abs=1e-2)