Camera

def world2image(P_world, cam2world, sensor_size, image_size, focal_length, image_center=None, kappa=0.0):
    """
    Project 3D world points to 2D image coordinates.
    
    Parameters:
    - P_world: array, shape (n_points, 4) - Points in world coordinates  
    - cam2world: array, shape (4, 4) - Camera pose transformation
    - sensor_size: array, shape (2,) - Physical sensor dimensions [width, height]
    - image_size: array, shape (2,) - Image dimensions in pixels [width, height]
    - focal_length: float - Camera focal length
    - image_center: array, shape (2,), optional - Image center offset
    - kappa: float, optional - Radial distortion parameter
    
    Returns:
    - P_image: array, shape (n_points, 2) - Image coordinates
    """

def cam2sensor(P_cam, focal_length, kappa=0.0):
    """
    Project 3D camera coordinates to 2D sensor plane.
    
    Parameters:
    - P_cam: array, shape (n_points, 3 or 4) - Points in camera coordinates
    - focal_length: float - Camera focal length  
    - kappa: float, optional - Radial distortion parameter
    
    Returns:
    - P_sensor: array, shape (n_points, 2) - Points on sensor plane
    """

def sensor2img(P_sensor, sensor_size, image_size, image_center=None):
    """
    Convert sensor coordinates to image pixel coordinates.
    
    Parameters:
    - P_sensor: array, shape (n_points, 2) - Points on sensor plane
    - sensor_size: array, shape (2,) - Physical sensor size [width, height]
    - image_size: array, shape (2,) - Image size in pixels [width, height]
    - image_center: array, shape (2,), optional - Image center offset
    
    Returns:
    - P_image: array, shape (n_points, 2) - Image pixel coordinates
    """

def make_world_grid(n_lines=11, n_points_per_line=51, xlim=(-0.5, 0.5), ylim=(-0.5, 0.5)):
    """
    Generate grid in world coordinate frame on x-y plane (z=0).
    
    Parameters:
    - n_lines: int - Number of grid lines in each direction
    - n_points_per_line: int - Points per grid line
    - xlim: tuple - X-axis limits (min, max)
    - ylim: tuple - Y-axis limits (min, max)
    
    Returns:
    - grid: array, shape (2*n_lines*n_points_per_line, 4) - Grid points as homogeneous coordinates
    """

def make_world_line(p1, p2, n_points):
    """
    Generate line between two points in world coordinates.
    
    Parameters:
    - p1: array-like, shape (2 or 3,) - Start point
    - p2: array-like, shape (2 or 3,) - End point  
    - n_points: int - Number of points along line
    
    Returns:
    - line: array, shape (n_points, 4) - Line points as homogeneous coordinates
    """

def plot_camera(ax=None, M=None, cam2world=None, virtual_image_distance=1.0, sensor_size=(1920, 1080), ax_s=1, strict_check=True, **kwargs):
    """
    Plot camera in 3D world coordinates with virtual image plane.
    
    Parameters:
    - ax: Axes3D, optional - Matplotlib 3D axis
    - M: array, shape (3, 4), optional - Camera matrix  
    - cam2world: array, shape (4, 4), optional - Camera pose
    - virtual_image_distance: float - Distance to virtual image plane
    - sensor_size: tuple - Sensor dimensions for visualization
    - ax_s: float - Axis scaling factor
    - strict_check: bool - Enable strict validation
    
    Returns:
    - ax: Axes3D - Matplotlib 3D axis with camera plot
    """

import numpy as np
import pytransform3d.camera as pc
import pytransform3d.transformations as pt

# Define camera parameters
focal_length = 0.05  # 50mm lens
sensor_size = np.array([0.036, 0.024])  # Full frame sensor
image_size = np.array([3840, 2560])  # 4K image

# Camera pose (looking down negative z-axis)
cam2world = pt.transform_from(p=[0, 0, 2])

# Create 3D world points
world_points = np.array([
    [0, 0, 0, 1],      # origin
    [1, 0, 0, 1],      # x-axis
    [0, 1, 0, 1],      # y-axis
    [0, 0, 1, 1],      # z-axis
])

# Project to image
image_points = pc.world2image(
    world_points, cam2world, sensor_size, image_size, focal_length
)

print("World points projected to image:")
for i, (world_pt, img_pt) in enumerate(zip(world_points, image_points)):
    print(f"  {world_pt[:3]} -> {img_pt}")

import pytransform3d.camera as pc
import pytransform3d.transformations as pt

# Define world points and camera
world_points = np.array([[1, 1, 0, 1], [2, 1, 0, 1]])
cam2world = pt.transform_from(p=[0, 0, 3])

# Step 1: Transform to camera coordinates
world2cam = pt.invert_transform(cam2world)
cam_points = pt.transform(world2cam, world_points)

# Step 2: Project to sensor plane
sensor_points = pc.cam2sensor(cam_points, focal_length=0.05)

# Step 3: Convert to image pixels
sensor_size = np.array([0.036, 0.024])
image_size = np.array([1920, 1080])
image_points = pc.sensor2img(sensor_points, sensor_size, image_size)

print(f"Sensor coordinates: {sensor_points}")
print(f"Image coordinates: {image_points}")

import matplotlib.pyplot as plt
import pytransform3d.camera as pc
import pytransform3d.transformations as pt
from pytransform3d.plot_utils import make_3d_axis

# Create cameras at different poses
cameras = [
    pt.transform_from(p=[2, 0, 1]),
    pt.transform_from(p=[0, 2, 1]),
    pt.transform_from(p=[-2, 0, 1]),
]

# Plot scene
ax = make_3d_axis(ax_s=3)

# Plot coordinate origin
ax.scatter([0], [0], [0], c='red', s=100, label='Origin')

# Plot cameras
for i, cam_pose in enumerate(cameras):
    pc.plot_camera(ax=ax, cam2world=cam_pose, virtual_image_distance=0.5)

ax.set_xlabel('X')
ax.set_ylabel('Y')  
ax.set_zlabel('Z')
ax.legend()
plt.show()

import pytransform3d.camera as pc
import matplotlib.pyplot as plt

# Generate calibration grid
grid = pc.make_world_grid(n_lines=10, n_points_per_line=10, 
                         xlim=(-0.5, 0.5), ylim=(-0.5, 0.5))

# Plot grid in 3D
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(grid[:, 0], grid[:, 1], grid[:, 2], s=1)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.show()

# Project grid to image
cam2world = pt.transform_from(p=[0, 0, 1])
image_points = pc.world2image(grid, cam2world, 
                             np.array([0.036, 0.024]), 
                             np.array([1920, 1080]), 
                             0.05)

# Plot projected grid
plt.figure()
plt.scatter(image_points[:, 0], image_points[:, 1], s=1)
plt.xlabel('Image X (pixels)')
plt.ylabel('Image Y (pixels)')
plt.title('Projected Calibration Grid')
plt.show()