geocalib/lm_optimizer.py

"""Implementation of the Levenberg-Marquardt optimizer for camera calibration."""

import logging
import time
from types import SimpleNamespace
from typing import Any, Callable, Dict, Tuple

import torch
import torch.nn as nn

from geocalib.camera import BaseCamera, camera_models
from geocalib.gravity import Gravity
from geocalib.misc import J_focal2fov
from geocalib.perspective_fields import J_perspective_field, get_perspective_field
from geocalib.utils import focal2fov, rad2deg

logger = logging.getLogger(__name__)


def get_trivial_estimation(data: Dict[str, torch.Tensor], camera_model: BaseCamera) -> BaseCamera:
    """Get initial camera for optimization with roll=0, pitch=0, vfov=0.7 * max(h, w).

    Args:
        data (Dict[str, torch.Tensor]): Input data dictionary.
        camera_model (BaseCamera): Camera model to use.

    Returns:
        BaseCamera: Initial camera for optimization.
    """
    """Get initial camera for optimization with roll=0, pitch=0, vfov=0.7 * max(h, w)."""
    ref = data.get("up_field", data["latitude_field"])
    ref = ref.detach()

    h, w = ref.shape[-2:]
    batch_h, batch_w = (
        ref.new_ones((ref.shape[0],)) * h,
        ref.new_ones((ref.shape[0],)) * w,
    )

    init_r = ref.new_zeros((ref.shape[0],))
    init_p = ref.new_zeros((ref.shape[0],))

    focal = data.get("prior_focal", 0.7 * torch.max(batch_h, batch_w))
    init_vfov = focal2fov(focal, h)

    params = {"width": batch_w, "height": batch_h, "vfov": init_vfov}
    params |= {"scales": data["scales"]} if "scales" in data else {}
    params |= {"k1": data["prior_k1"]} if "prior_k1" in data else {}
    camera = camera_model.from_dict(params)
    camera = camera.float().to(ref.device)

    gravity = Gravity.from_rp(init_r, init_p).float().to(ref.device)

    if "prior_gravity" in data:
        gravity = data["prior_gravity"].float().to(ref.device)
        gravity = Gravity(gravity) if isinstance(gravity, torch.Tensor) else gravity

    return camera, gravity


def scaled_loss(
    x: torch.Tensor, fn: Callable, a: float
) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
    """Apply a loss function to a tensor and pre- and post-scale it.

    Args:
        x: the data tensor, should already be squared: `x = y**2`.
        fn: the loss function, with signature `fn(x) -> y`.
        a: the scale parameter.

    Returns:
        The value of the loss, and its first and second derivatives.
    """
    a2 = a**2
    loss, loss_d1, loss_d2 = fn(x / a2)
    return loss * a2, loss_d1, loss_d2 / a2


def huber_loss(x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
    """The classical robust Huber loss, with first and second derivatives."""
    mask = x <= 1
    sx = torch.sqrt(x + 1e-8)  # avoid nan in backward pass
    isx = torch.max(sx.new_tensor(torch.finfo(torch.float).eps), 1 / sx)
    loss = torch.where(mask, x, 2 * sx - 1)
    loss_d1 = torch.where(mask, torch.ones_like(x), isx)
    loss_d2 = torch.where(mask, torch.zeros_like(x), -isx / (2 * x))
    return loss, loss_d1, loss_d2


def early_stop(new_cost: torch.Tensor, prev_cost: torch.Tensor, atol: float, rtol: float) -> bool:
    """Early stopping criterion based on cost convergence."""
    return torch.allclose(new_cost, prev_cost, atol=atol, rtol=rtol)


def update_lambda(
    lamb: torch.Tensor,
    prev_cost: torch.Tensor,
    new_cost: torch.Tensor,
    lambda_min: float = 1e-6,
    lambda_max: float = 1e2,
) -> torch.Tensor:
    """Update damping factor for Levenberg-Marquardt optimization."""
    new_lamb = lamb.new_zeros(lamb.shape)
    new_lamb = lamb * torch.where(new_cost > prev_cost, 10, 0.1)
    lamb = torch.clamp(new_lamb, lambda_min, lambda_max)
    return lamb


def optimizer_step(
    G: torch.Tensor, H: torch.Tensor, lambda_: torch.Tensor, eps: float = 1e-6
) -> torch.Tensor:
    """One optimization step with Gauss-Newton or Levenberg-Marquardt.

    Args:
        G (torch.Tensor): Batched gradient tensor of size (..., N).
        H (torch.Tensor): Batched hessian tensor of size (..., N, N).
        lambda_ (torch.Tensor): Damping factor for LM (use GN if lambda_=0) with shape (B,).
        eps (float, optional): Epsilon for damping. Defaults to 1e-6.

    Returns:
        torch.Tensor: Batched update tensor of size (..., N).
    """
    diag = H.diagonal(dim1=-2, dim2=-1)
    diag = diag * lambda_.unsqueeze(-1)  # (B, 3)

    H = H + diag.clamp(min=eps).diag_embed()

    H_, G_ = H.cpu(), G.cpu()
    try:
        U = torch.linalg.cholesky(H_)
    except RuntimeError:
        logger.warning("Cholesky decomposition failed. Stopping.")
        delta = H.new_zeros((H.shape[0], H.shape[-1]))  # (B, 3)
    else:
        delta = torch.cholesky_solve(G_[..., None], U)[..., 0]

    return delta.to(H.device)


# mypy: ignore-errors
class LMOptimizer(nn.Module):
    """Levenberg-Marquardt optimizer for camera calibration."""

    default_conf = {
        # Camera model parameters
        "camera_model": "pinhole",  # {"pinhole", "simple_radial", "simple_spherical"}
        "shared_intrinsics": False,  # share focal length across all images in batch
        # LM optimizer parameters
        "num_steps": 30,
        "lambda_": 0.1,
        "fix_lambda": False,
        "early_stop": True,
        "atol": 1e-8,
        "rtol": 1e-8,
        "use_spherical_manifold": True,  # use spherical manifold for gravity optimization
        "use_log_focal": True,  # use log focal length for optimization
        # Loss function parameters
        "up_loss_fn_scale": 1e-2,
        "lat_loss_fn_scale": 1e-2,
        # Misc
        "verbose": False,
    }

    def __init__(self, conf: Dict[str, Any]):
        """Initialize the LM optimizer."""
        super().__init__()
        self.conf = conf = SimpleNamespace(**{**self.default_conf, **conf})
        self.num_steps = conf.num_steps

        self.set_camera_model(conf.camera_model)
        self.setup_optimization_and_priors(shared_intrinsics=conf.shared_intrinsics)

    def set_camera_model(self, camera_model: str) -> None:
        """Set the camera model to use for the optimization.

        Args:
            camera_model (str): Camera model to use.
        """
        assert (
            camera_model in camera_models.keys()
        ), f"Unknown camera model: {camera_model} not in {camera_models.keys()}"
        self.camera_model = camera_models[camera_model]
        self.camera_has_distortion = hasattr(self.camera_model, "dist")

        logger.debug(
            f"Using camera model: {camera_model} (with distortion: {self.camera_has_distortion})"
        )

    def setup_optimization_and_priors(
        self, data: Dict[str, torch.Tensor] = None, shared_intrinsics: bool = False
    ) -> None:
        """Setup the optimization and priors for the LM optimizer.

        Args:
            data (Dict[str, torch.Tensor], optional): Dict potentially containing priors. Defaults
            to None.
            shared_intrinsics (bool, optional): Whether to share the intrinsics across the batch.
            Defaults to False.
        """
        if data is None:
            data = {}
        self.shared_intrinsics = shared_intrinsics

        if shared_intrinsics:  # si => must use pinhole
            assert (
                self.camera_model == camera_models["pinhole"]
            ), f"Shared intrinsics only supported with pinhole camera model: {self.camera_model}"

        self.estimate_gravity = True
        if "prior_gravity" in data:
            self.estimate_gravity = False
            logger.debug("Using provided gravity as prior.")

        self.estimate_focal = True
        if "prior_focal" in data:
            self.estimate_focal = False
            logger.debug("Using provided focal as prior.")

        self.estimate_k1 = True
        if "prior_k1" in data:
            self.estimate_k1 = False
            logger.debug("Using provided k1 as prior.")

        self.gravity_delta_dims = (0, 1) if self.estimate_gravity else (-1,)
        self.focal_delta_dims = (
            (max(self.gravity_delta_dims) + 1,) if self.estimate_focal else (-1,)
        )
        self.k1_delta_dims = (max(self.focal_delta_dims) + 1,) if self.estimate_k1 else (-1,)

        logger.debug(f"Camera Model:       {self.camera_model}")
        logger.debug(f"Optimizing gravity: {self.estimate_gravity} ({self.gravity_delta_dims})")
        logger.debug(f"Optimizing focal:   {self.estimate_focal} ({self.focal_delta_dims})")
        logger.debug(f"Optimizing k1:      {self.estimate_k1} ({self.k1_delta_dims})")

        logger.debug(f"Shared intrinsics:  {self.shared_intrinsics}")

    def calculate_residuals(
        self, camera: BaseCamera, gravity: Gravity, data: Dict[str, torch.Tensor]
    ) -> Dict[str, torch.Tensor]:
        """Calculate the residuals for the optimization.

        Args:
            camera (BaseCamera): Optimized camera.
            gravity (Gravity): Optimized gravity.
            data (Dict[str, torch.Tensor]): Input data containing the up and latitude fields.

        Returns:
            Dict[str, torch.Tensor]: Residuals for the optimization.
        """
        perspective_up, perspective_lat = get_perspective_field(camera, gravity)
        perspective_lat = torch.sin(perspective_lat)

        residuals = {}
        if "up_field" in data:
            up_residual = (data["up_field"] - perspective_up).permute(0, 2, 3, 1)
            residuals["up_residual"] = up_residual.reshape(up_residual.shape[0], -1, 2)

        if "latitude_field" in data:
            target_lat = torch.sin(data["latitude_field"])
            lat_residual = (target_lat - perspective_lat).permute(0, 2, 3, 1)
            residuals["latitude_residual"] = lat_residual.reshape(lat_residual.shape[0], -1, 1)

        return residuals

    def calculate_costs(
        self, residuals: torch.Tensor, data: Dict[str, torch.Tensor]
    ) -> Tuple[Dict[str, torch.Tensor], Dict[str, torch.Tensor]]:
        """Calculate the costs and weights for the optimization.

        Args:
            residuals (torch.Tensor): Residuals for the optimization.
            data (Dict[str, torch.Tensor]): Input data containing the up and latitude confidence.

        Returns:
            Tuple[Dict[str, torch.Tensor], Dict[str, torch.Tensor]]: Costs and weights for the
            optimization.
        """
        costs, weights = {}, {}

        if "up_residual" in residuals:
            up_cost = (residuals["up_residual"] ** 2).sum(dim=-1)
            up_cost, up_weight, _ = scaled_loss(up_cost, huber_loss, self.conf.up_loss_fn_scale)

            if "up_confidence" in data:
                up_conf = data["up_confidence"].reshape(up_weight.shape[0], -1)
                up_weight = up_weight * up_conf
                up_cost = up_cost * up_conf

            costs["up_cost"] = up_cost
            weights["up_weights"] = up_weight

        if "latitude_residual" in residuals:
            lat_cost = (residuals["latitude_residual"] ** 2).sum(dim=-1)
            lat_cost, lat_weight, _ = scaled_loss(lat_cost, huber_loss, self.conf.lat_loss_fn_scale)

            if "latitude_confidence" in data:
                lat_conf = data["latitude_confidence"].reshape(lat_weight.shape[0], -1)
                lat_weight = lat_weight * lat_conf
                lat_cost = lat_cost * lat_conf

            costs["latitude_cost"] = lat_cost
            weights["latitude_weights"] = lat_weight

        return costs, weights

    def calculate_gradient_and_hessian(
        self,
        J: torch.Tensor,
        residuals: torch.Tensor,
        weights: torch.Tensor,
        shared_intrinsics: bool,
    ) -> Tuple[torch.Tensor, torch.Tensor]:
        """Calculate the gradient and Hessian for given the Jacobian, residuals, and weights.

        Args:
            J (torch.Tensor): Jacobian.
            residuals (torch.Tensor): Residuals.
            weights (torch.Tensor): Weights.
            shared_intrinsics (bool): Whether to share the intrinsics across the batch.

        Returns:
            Tuple[torch.Tensor, torch.Tensor]: Gradient and Hessian.
        """
        dims = ()
        if self.estimate_gravity:
            dims = (0, 1)
        if self.estimate_focal:
            dims += (2,)
        if self.camera_has_distortion and self.estimate_k1:
            dims += (3,)
        assert dims, "No parameters to optimize"

        J = J[..., dims]

        Grad = torch.einsum("...Njk,...Nj->...Nk", J, residuals)
        Grad = weights[..., None] * Grad
        Grad = Grad.sum(-2)  # (B, N_params)

        if shared_intrinsics:
            # reshape to (1, B * (N_params-1) + 1)
            Grad_g = Grad[..., :2].reshape(1, -1)
            Grad_f = Grad[..., 2].reshape(1, -1).sum(-1, keepdim=True)
            Grad = torch.cat([Grad_g, Grad_f], dim=-1)

        Hess = torch.einsum("...Njk,...Njl->...Nkl", J, J)
        Hess = weights[..., None, None] * Hess
        Hess = Hess.sum(-3)

        if shared_intrinsics:
            H_g = torch.block_diag(*list(Hess[..., :2, :2]))
            J_fg = Hess[..., :2, 2].flatten()
            J_gf = Hess[..., 2, :2].flatten()
            J_f = Hess[..., 2, 2].sum()
            dims = H_g.shape[-1] + 1
            Hess = Hess.new_zeros((dims, dims), dtype=torch.float32)
            Hess[:-1, :-1] = H_g
            Hess[-1, :-1] = J_gf
            Hess[:-1, -1] = J_fg
            Hess[-1, -1] = J_f
            Hess = Hess.unsqueeze(0)

        return Grad, Hess

    def setup_system(
        self,
        camera: BaseCamera,
        gravity: Gravity,
        residuals: Dict[str, torch.Tensor],
        weights: Dict[str, torch.Tensor],
        as_rpf: bool = False,
        shared_intrinsics: bool = False,
    ) -> Tuple[torch.Tensor, torch.Tensor]:
        """Calculate the gradient and Hessian for the optimization.

        Args:
            camera (BaseCamera): Optimized camera.
            gravity (Gravity): Optimized gravity.
            residuals (Dict[str, torch.Tensor]): Residuals for the optimization.
            weights (Dict[str, torch.Tensor]): Weights for the optimization.
            as_rpf (bool, optional): Wether to calculate the gradient and Hessian with respect to
            roll, pitch, and focal length. Defaults to False.
            shared_intrinsics (bool, optional): Whether to share the intrinsics across the batch.
            Defaults to False.

        Returns:
            Tuple[torch.Tensor, torch.Tensor]: Gradient and Hessian for the optimization.
        """
        J_up, J_lat = J_perspective_field(
            camera,
            gravity,
            spherical=self.conf.use_spherical_manifold and not as_rpf,
            log_focal=self.conf.use_log_focal and not as_rpf,
        )

        J_up = J_up.reshape(J_up.shape[0], -1, J_up.shape[-2], J_up.shape[-1])  # (B, N, 2, 3)
        J_lat = J_lat.reshape(J_lat.shape[0], -1, J_lat.shape[-2], J_lat.shape[-1])  # (B, N, 1, 3)

        n_params = (
            2 * self.estimate_gravity
            + self.estimate_focal
            + (self.camera_has_distortion and self.estimate_k1)
        )
        Grad = J_up.new_zeros(J_up.shape[0], n_params)
        Hess = J_up.new_zeros(J_up.shape[0], n_params, n_params)

        if shared_intrinsics:
            N_params = Grad.shape[0] * (n_params - 1) + 1
            Grad = Grad.new_zeros(1, N_params)
            Hess = Hess.new_zeros(1, N_params, N_params)

        if "up_residual" in residuals:
            Up_Grad, Up_Hess = self.calculate_gradient_and_hessian(
                J_up, residuals["up_residual"], weights["up_weights"], shared_intrinsics
            )

            if self.conf.verbose:
                logger.info(f"Up J:\n{Up_Grad.mean(0)}")

            Grad = Grad + Up_Grad
            Hess = Hess + Up_Hess

        if "latitude_residual" in residuals:
            Lat_Grad, Lat_Hess = self.calculate_gradient_and_hessian(
                J_lat,
                residuals["latitude_residual"],
                weights["latitude_weights"],
                shared_intrinsics,
            )

            if self.conf.verbose:
                logger.info(f"Lat J:\n{Lat_Grad.mean(0)}")

            Grad = Grad + Lat_Grad
            Hess = Hess + Lat_Hess

        return Grad, Hess

    def estimate_uncertainty(
        self,
        camera_opt: BaseCamera,
        gravity_opt: Gravity,
        errors: Dict[str, torch.Tensor],
        weights: Dict[str, torch.Tensor],
    ) -> Dict[str, torch.Tensor]:
        """Estimate the uncertainty of the optimized camera and gravity at the final step.

        Args:
            camera_opt (BaseCamera): Final optimized camera.
            gravity_opt (Gravity): Final optimized gravity.
            errors (Dict[str, torch.Tensor]): Costs for the optimization.
            weights (Dict[str, torch.Tensor]): Weights for the optimization.

        Returns:
            Dict[str, torch.Tensor]: Uncertainty estimates for the optimized camera and gravity.
        """
        _, Hess = self.setup_system(
            camera_opt, gravity_opt, errors, weights, as_rpf=True, shared_intrinsics=False
        )
        Cov = torch.inverse(Hess)

        roll_uncertainty = Cov.new_zeros(Cov[..., 0, 0].shape)
        pitch_uncertainty = Cov.new_zeros(Cov[..., 0, 0].shape)
        gravity_uncertainty = Cov.new_zeros(Cov[..., 0, 0].shape)
        if self.estimate_gravity:
            roll_uncertainty = Cov[..., 0, 0]
            pitch_uncertainty = Cov[..., 1, 1]

            try:
                delta_uncertainty = Cov[..., :2, :2]
                eigenvalues = torch.linalg.eigvalsh(delta_uncertainty.cpu())
                gravity_uncertainty = torch.max(eigenvalues, dim=-1).values.to(Cov.device)
            except RuntimeError:
                logger.warning("Could not calculate gravity uncertainty")
                gravity_uncertainty = Cov.new_zeros(Cov.shape[0])

        focal_uncertainty = Cov.new_zeros(Cov[..., 0, 0].shape)
        fov_uncertainty = Cov.new_zeros(Cov[..., 0, 0].shape)
        if self.estimate_focal:
            focal_uncertainty = Cov[..., self.focal_delta_dims[0], self.focal_delta_dims[0]]
            fov_uncertainty = (
                J_focal2fov(camera_opt.f[..., 1], camera_opt.size[..., 1]) ** 2 * focal_uncertainty
            )

        return {
            "covariance": Cov,
            "roll_uncertainty": torch.sqrt(roll_uncertainty),
            "pitch_uncertainty": torch.sqrt(pitch_uncertainty),
            "gravity_uncertainty": torch.sqrt(gravity_uncertainty),
            "focal_uncertainty": torch.sqrt(focal_uncertainty) / 2,
            "vfov_uncertainty": torch.sqrt(fov_uncertainty / 2),
        }

    def update_estimate(
        self, camera: BaseCamera, gravity: Gravity, delta: torch.Tensor
    ) -> Tuple[BaseCamera, Gravity]:
        """Update the camera and gravity estimates with the given delta.

        Args:
            camera (BaseCamera): Optimized camera.
            gravity (Gravity): Optimized gravity.
            delta (torch.Tensor): Delta to update the camera and gravity estimates.

        Returns:
            Tuple[BaseCamera, Gravity]: Updated camera and gravity estimates.
        """
        delta_gravity = (
            delta[..., self.gravity_delta_dims]
            if self.estimate_gravity
            else delta.new_zeros(delta.shape[:-1] + (2,))
        )
        new_gravity = gravity.update(delta_gravity, spherical=self.conf.use_spherical_manifold)

        delta_f = (
            delta[..., self.focal_delta_dims]
            if self.estimate_focal
            else delta.new_zeros(delta.shape[:-1] + (1,))
        )
        new_camera = camera.update_focal(delta_f, as_log=self.conf.use_log_focal)

        delta_dist = (
            delta[..., self.k1_delta_dims]
            if self.camera_has_distortion and self.estimate_k1
            else delta.new_zeros(delta.shape[:-1] + (1,))
        )
        if self.camera_has_distortion:
            new_camera = new_camera.update_dist(delta_dist)

        return new_camera, new_gravity

    def optimize(
        self,
        data: Dict[str, torch.Tensor],
        camera_opt: BaseCamera,
        gravity_opt: Gravity,
    ) -> Tuple[BaseCamera, Gravity, Dict[str, torch.Tensor]]:
        """Optimize the camera and gravity estimates.

        Args:
            data (Dict[str, torch.Tensor]): Input data.
            camera_opt (BaseCamera): Optimized camera.
            gravity_opt (Gravity): Optimized gravity.

        Returns:
            Tuple[BaseCamera, Gravity, Dict[str, torch.Tensor]]: Optimized camera, gravity
            estimates and optimization information.
        """
        key = list(data.keys())[0]
        B = data[key].shape[0]

        lamb = data[key].new_ones(B) * self.conf.lambda_
        if self.shared_intrinsics:
            lamb = data[key].new_ones(1) * self.conf.lambda_

        infos = {"stop_at": self.num_steps}
        for i in range(self.num_steps):
            if self.conf.verbose:
                logger.info(f"Step {i+1}/{self.num_steps}")

            errors = self.calculate_residuals(camera_opt, gravity_opt, data)
            costs, weights = self.calculate_costs(errors, data)

            if i == 0:
                prev_cost = sum(c.mean(-1) for c in costs.values())
                for k, c in costs.items():
                    infos[f"initial_{k}"] = c.mean(-1)

                infos["initial_cost"] = prev_cost

            Grad, Hess = self.setup_system(
                camera_opt,
                gravity_opt,
                errors,
                weights,
                shared_intrinsics=self.shared_intrinsics,
            )
            delta = optimizer_step(Grad, Hess, lamb)  # (B, N_params)

            if self.shared_intrinsics:
                delta_g = delta[..., :-1].reshape(B, 2)
                delta_f = delta[..., -1].expand(B, 1)
                delta = torch.cat([delta_g, delta_f], dim=-1)

            # calculate new cost
            camera_opt, gravity_opt = self.update_estimate(camera_opt, gravity_opt, delta)
            new_cost, _ = self.calculate_costs(
                self.calculate_residuals(camera_opt, gravity_opt, data), data
            )
            new_cost = sum(c.mean(-1) for c in new_cost.values())

            if not self.conf.fix_lambda and not self.shared_intrinsics:
                lamb = update_lambda(lamb, prev_cost, new_cost)

            if self.conf.verbose:
                logger.info(f"Cost:\nPrev: {prev_cost}\nNew:  {new_cost}")
                logger.info(f"Camera:\n{camera_opt._data}")

            if early_stop(new_cost, prev_cost, atol=self.conf.atol, rtol=self.conf.rtol):
                infos["stop_at"] = min(i + 1, infos["stop_at"])

                if self.conf.early_stop:
                    if self.conf.verbose:
                        logger.info(f"Early stopping at step {i+1}")
                    break

            prev_cost = new_cost

            if i == self.num_steps - 1 and self.conf.early_stop:
                logger.warning("Reached maximum number of steps without convergence.")

        final_errors = self.calculate_residuals(camera_opt, gravity_opt, data)  # (B, N, 3)
        final_cost, weights = self.calculate_costs(final_errors, data)  # (B, N)

        if not self.training:
            infos |= self.estimate_uncertainty(camera_opt, gravity_opt, final_errors, weights)

        infos["stop_at"] = camera_opt.new_ones(camera_opt.shape[0]) * infos["stop_at"]
        for k, c in final_cost.items():
            infos[f"final_{k}"] = c.mean(-1)

        infos["final_cost"] = sum(c.mean(-1) for c in final_cost.values())

        return camera_opt, gravity_opt, infos

    def forward(self, data: Dict[str, torch.Tensor]) -> Dict[str, torch.Tensor]:
        """Run the LM optimization."""
        camera_init, gravity_init = get_trivial_estimation(data, self.camera_model)

        self.setup_optimization_and_priors(data, shared_intrinsics=self.shared_intrinsics)

        start = time.time()
        camera_opt, gravity_opt, infos = self.optimize(data, camera_init, gravity_init)

        if self.conf.verbose:
            logger.info(f"Optimization took {(time.time() - start)*1000:.2f} ms")

            logger.info(f"Initial camera:\n{rad2deg(camera_init.vfov)}")
            logger.info(f"Optimized camera:\n{rad2deg(camera_opt.vfov)}")

            logger.info(f"Initial gravity:\n{rad2deg(gravity_init.rp)}")
            logger.info(f"Optimized gravity:\n{rad2deg(gravity_opt.rp)}")

        return {"camera": camera_opt, "gravity": gravity_opt, **infos}