def fit_continuous_trajectory(
    problem: ContinuousTimeTrajectoryFitProblem,
    options: ContinuousTimeTrajectoryFitOptions | None = None,
) -> ContinuousTimeTrajectoryFitResult:
    """Minimize ``sum(||weight * residual||^2)`` plus information-form priors."""

    settings = options or ContinuousTimeTrajectoryFitOptions()
    timestamps = problem.knot_timestamps
    knots = list(problem.initial_knot_poses)
    gyro_bias = np.asarray(problem.initial_gyro_bias_rad_s, dtype=float)
    lever_arm = np.asarray(problem.initial_lever_arm_body_m, dtype=float)
    clock_offset = float(problem.initial_imu_clock_offset_sec)
    accel_bias = np.asarray(problem.initial_accel_bias_body_m_s2, dtype=float)
    gravity = np.asarray(problem.initial_gravity_world_m_s2, dtype=float)
    gyro_scale = np.asarray(problem.initial_gyro_scale, dtype=float)
    accel_scale = np.asarray(problem.initial_accel_scale, dtype=float)
    imu_rotation_vector = np.asarray(problem.initial_imu_rotation_vector_rad, dtype=float)
    estimate_bias = problem.estimate_gyro_bias
    estimate_lever = problem.estimate_lever_arm
    estimate_clock = problem.estimate_imu_clock_offset
    estimate_accel = problem.estimate_accel_bias
    estimate_grav = problem.estimate_gravity
    estimate_gyro_scale = problem.estimate_gyro_scale
    estimate_accel_scale = problem.estimate_accel_scale
    estimate_imu_rot = problem.estimate_imu_rotation
    point_rows: list[tuple[int, tuple[float, float, float]]] = []
    plane_rows: list[tuple[int, TrajectoryPointToPlaneMeasurement]] = []
    pose_rows: list[tuple[int, SE3]] = []
    imu_rows: list[TrajectoryImuPreintegrationMeasurement] = []
    lever_rows: list[TrajectoryImuLeverArmMeasurement] = []
    for point_measurement in problem.point_measurements:
        interval = _interval_index(timestamps, point_measurement.timestamp_sec)
        if interval is None:
            raise ValueError(
                f"point measurement {point_measurement.measurement_id!r} is outside "
                "the knot domain"
            )
        point_rows.append((interval, point_measurement.point_body_m))
    for plane_measurement in problem.point_to_plane_measurements:
        interval = _interval_index(timestamps, plane_measurement.timestamp_sec)
        if interval is None:
            raise ValueError(
                f"point-to-plane measurement {plane_measurement.measurement_id!r} is "
                "outside the knot domain"
            )
        plane_rows.append((interval, plane_measurement))
    for pose_measurement in problem.pose_measurements:
        interval = _interval_index(timestamps, pose_measurement.timestamp_sec)
        if interval is None:
            raise ValueError(
                f"pose measurement {pose_measurement.measurement_id!r} is outside "
                "the knot domain"
            )
        pose_rows.append((interval, pose_measurement.pose_world_body))
    for imu_measurement in problem.imu_preintegration_measurements:
        if (
            _interval_index(
                timestamps,
                _imu_body_time(imu_measurement.start_time_sec, clock_offset),
            )
            is None
            or _interval_index(
                timestamps,
                _imu_body_time(imu_measurement.end_time_sec, clock_offset),
            )
            is None
        ):
            raise ValueError(
                f"IMU pre-integration {imu_measurement.measurement_id!r} is outside "
                "the knot domain"
            )
        imu_rows.append(imu_measurement)
    for lever_measurement in problem.imu_lever_arm_measurements:
        if _interval_index(
            timestamps,
            _imu_body_time(lever_measurement.timestamp_sec, clock_offset),
        ) is None:
            raise ValueError(
                f"lever-arm measurement {lever_measurement.measurement_id!r} is outside "
                "the knot domain"
            )
        lever_rows.append(lever_measurement)

    damping = settings.initial_damping
    gradient_norm = math.inf
    objective = _objective(
        problem,
        point_rows,
        plane_rows,
        pose_rows,
        imu_rows,
        lever_rows,
        knots,
        gyro_bias,
        lever_arm,
        clock_offset,
        accel_bias,
        gravity,
        gyro_scale,
        accel_scale,
        imu_rotation=_imu_rotation_matrix(imu_rotation_vector),
    )
    best_objective = objective
    best_knots = list(knots)
    best_bias = np.array(gyro_bias, copy=True)
    best_lever = np.array(lever_arm, copy=True)
    best_clock = clock_offset
    best_accel = np.array(accel_bias, copy=True)
    best_gravity = np.array(gravity, copy=True)
    best_gyro_scale = np.array(gyro_scale, copy=True)
    best_accel_scale = np.array(accel_scale, copy=True)
    best_imu_rotation = np.array(imu_rotation_vector, copy=True)
    max_step_translation = 0.0
    max_step_rotation_deg = 0.0
    status: FitStatus = "max_iterations"
    iterations = 0
    knot_count = len(knots)
    parameter_count = (
        6 * knot_count
        + (3 if estimate_bias else 0)
        + (3 if estimate_lever else 0)
        + (1 if estimate_clock else 0)
        + (3 if estimate_accel else 0)
        + (3 if estimate_grav else 0)
        + (3 if estimate_gyro_scale else 0)
        + (3 if estimate_accel_scale else 0)
        + (3 if estimate_imu_rot else 0)
    )
    bias_offset = 6 * knot_count
    lever_offset = bias_offset + (3 if estimate_bias else 0)
    clock_offset_index = lever_offset + (3 if estimate_lever else 0)
    accel_offset = clock_offset_index + (1 if estimate_clock else 0)
    gravity_offset = accel_offset + (3 if estimate_accel else 0)
    gyro_scale_offset = gravity_offset + (3 if estimate_grav else 0)
    accel_scale_offset = gyro_scale_offset + (3 if estimate_gyro_scale else 0)
    imu_rotation_offset = accel_scale_offset + (3 if estimate_accel_scale else 0)
    blocks: list[tuple[int, int, NDArray[np.float64]]] = []

    def _linearize(
        knots: list[SE3],
        gyro_bias: FloatArray,
        lever_arm: FloatArray,
        clock_offset: float,
        accel_bias: FloatArray,
        gravity: FloatArray,
        gyro_scale: FloatArray,
        accel_scale: FloatArray,
        imu_rotation_vector: FloatArray,
    ) -> tuple[
        list[tuple[int, int, NDArray[np.float64]]], FloatArray, FloatArray, csc_matrix
    ]:
        imu_rotation = _imu_rotation_matrix(imu_rotation_vector)
        blocks: list[tuple[int, int, NDArray[np.float64]]] = []
        residuals: list[float] = []
        weights: list[float] = []
        row_offset = 0
        for index, (interval, point_body) in enumerate(point_rows):
            measurement = problem.point_measurements[index]
            residual, jacobian_left, jacobian_right = _point_residual(
                knots,
                timestamps,
                interval,
                measurement.timestamp_sec,
                point_body,
                measurement.target_world_m,
            )
            residuals.extend(float(value) for value in residual)
            weights.extend([measurement.weight] * 3)
            blocks.append((row_offset, 6 * interval, jacobian_left))
            blocks.append((row_offset, 6 * (interval + 1), jacobian_right))
            row_offset += 3
        for interval, plane_measurement in plane_rows:
            residual, jacobian_left, jacobian_right = _point_to_plane_residual(
                knots,
                timestamps,
                interval,
                plane_measurement,
            )
            residuals.append(float(residual[0]))
            weights.append(plane_measurement.weight)
            blocks.append((row_offset, 6 * interval, jacobian_left))
            blocks.append((row_offset, 6 * (interval + 1), jacobian_right))
            row_offset += 1
        for index, (interval, pose) in enumerate(pose_rows):
            weight = problem.pose_measurements[index].weight
            pose_measurement = problem.pose_measurements[index]
            for anchor in _ANCHOR_POINTS:
                residual, jacobian_left, jacobian_right = _anchor_residual(
                    knots,
                    timestamps,
                    interval,
                    pose_measurement.timestamp_sec,
                    anchor,
                    pose,
                )
                residuals.extend(float(value) for value in residual)
                weights.extend([weight] * 3)
                blocks.append((row_offset, 6 * interval, jacobian_left))
                blocks.append((row_offset, 6 * (interval + 1), jacobian_right))
                row_offset += 3
        for imu_measurement in imu_rows:
            (
                residual,
                knot_blocks,
                jacobian_bias,
                jacobian_clock,
                jacobian_gyro_scale,
            ) = _imu_preintegration_residual(
                knots,
                timestamps,
                imu_measurement,
                gyro_bias,
                clock_offset,
                gyro_scale,
                imu_rotation=imu_rotation,
            )
            residuals.extend(float(value) for value in residual)
            weights.extend([imu_measurement.weight] * 3)
            if estimate_imu_rot:
                blocks.append(
                    (
                        row_offset,
                        imu_rotation_offset,
                        _imu_preintegration_rotation_jacobian(
                            knots,
                            timestamps,
                            imu_measurement,
                            gyro_bias,
                            clock_offset,
                            gyro_scale,
                            _rotation_or_identity(imu_rotation),
                        ),
                    )
                )
            for knot_index, jacobian in knot_blocks:
                blocks.append((row_offset, 6 * knot_index, jacobian))
            if estimate_bias:
                blocks.append((row_offset, 6 * knot_count, jacobian_bias))
            if estimate_clock:
                blocks.append((row_offset, clock_offset_index, jacobian_clock))
            if estimate_gyro_scale:
                blocks.append((row_offset, gyro_scale_offset, jacobian_gyro_scale))
            row_offset += 3
        for lever_measurement in lever_rows:
            (
                residual,
                jacobian_lever,
                jacobian_clock,
                jacobian_accel,
                jacobian_gravity,
                jacobian_accel_scale,
            ) = _lever_arm_residual(
                knots,
                timestamps,
                lever_measurement,
                lever_arm,
                clock_offset,
                accel_bias,
                gravity,
                accel_scale,
                imu_rotation=imu_rotation,
            )
            residuals.extend(float(value) for value in residual)
            weights.extend([lever_measurement.weight] * 3)
            if estimate_imu_rot:
                blocks.append(
                    (
                        row_offset,
                        imu_rotation_offset,
                        _lever_arm_rotation_jacobian(
                            knots,
                            timestamps,
                            lever_measurement,
                            lever_arm,
                            clock_offset,
                            gravity,
                            accel_scale,
                            _rotation_or_identity(imu_rotation),
                        ),
                    )
                )
            if estimate_lever:
                blocks.append((row_offset, lever_offset, jacobian_lever))
            if estimate_clock:
                blocks.append((row_offset, clock_offset_index, jacobian_clock))
            if estimate_accel:
                blocks.append((row_offset, accel_offset, jacobian_accel))
            if estimate_grav:
                blocks.append((row_offset, gravity_offset, jacobian_gravity))
            if estimate_accel_scale:
                blocks.append((row_offset, accel_scale_offset, jacobian_accel_scale))
            for knot_index, jacobian in _lever_arm_knot_jacobians(
                knots,
                timestamps,
                lever_measurement,
                lever_arm,
                clock_offset,
                gravity,
                accel_scale,
                imu_rotation=imu_rotation,
            ):
                blocks.append((row_offset, 6 * knot_index, jacobian))
            row_offset += 3
        for prior in problem.knot_marginalization_priors:
            if prior.knot_index < 0 or prior.knot_index >= knot_count:
                raise ValueError("marginalization prior knot index is out of range")
            weighted_residual, weighted_jacobian = marginalization_prior_blocks(
                prior, knots[prior.knot_index]
            )
            for axis in range(6):
                residuals.append(float(weighted_residual[axis]))
                weights.append(1.0)
                blocks.append(
                    (
                        row_offset,
                        6 * prior.knot_index,
                        weighted_jacobian[axis : axis + 1],
                    )
                )
                row_offset += 1

        residual_array = np.asarray(residuals, dtype=float)
        weight_array = np.asarray(weights, dtype=float)
        jacobian = _assemble_sparse_jacobian(
            blocks,
            residual_count=len(residuals),
            parameter_count=parameter_count,
        )
        return blocks, residual_array, weight_array, jacobian

    for iteration in range(settings.max_iterations):
        iterations = iteration + 1
        blocks, residual_array, weight_array, jacobian = _linearize(
            knots,
            gyro_bias,
            lever_arm,
            clock_offset,
            accel_bias,
            gravity,
            gyro_scale,
            accel_scale,
            imu_rotation_vector,
        )
        weighted_jacobian = jacobian.multiply(weight_array[:, None])
        normal = weighted_jacobian.T @ weighted_jacobian
        rhs = weighted_jacobian.T @ (weight_array * residual_array)
        gradient_norm = float(np.max(np.abs(rhs)))

        if gradient_norm < settings.convergence_gradient_norm:
            status = "converged"
            break

        damping_matrix = normal + damping * _block_diagonal_scale(
            normal, knot_count, parameter_count
        )
        try:
            delta = spsolve(damping_matrix.tocsc(), -rhs)
        except RuntimeError:
            status = "singular_system"
            break
        if not np.all(np.isfinite(delta)):
            status = "singular_system"
            break

        candidate = [
            se3_exp(knot, delta[6 * i : 6 * i + 6]) for i, knot in enumerate(knots)
        ]
        candidate_bias = np.array(gyro_bias, copy=True)
        candidate_lever = np.array(lever_arm, copy=True)
        candidate_clock = clock_offset
        candidate_accel = np.array(accel_bias, copy=True)
        candidate_gravity = np.array(gravity, copy=True)
        candidate_gyro_scale = np.array(gyro_scale, copy=True)
        candidate_accel_scale = np.array(accel_scale, copy=True)
        candidate_imu_rotation = np.array(imu_rotation_vector, copy=True)
        if estimate_bias:
            candidate_bias = gyro_bias + np.asarray(
                delta[bias_offset : bias_offset + 3], dtype=float
            )
        if estimate_lever:
            candidate_lever = lever_arm + np.asarray(
                delta[lever_offset : lever_offset + 3], dtype=float
            )
        if estimate_clock:
            candidate_clock = clock_offset + float(delta[clock_offset_index])
        if estimate_accel:
            candidate_accel = accel_bias + np.asarray(
                delta[accel_offset : accel_offset + 3], dtype=float
            )
        if estimate_grav:
            candidate_gravity = gravity + np.asarray(
                delta[gravity_offset : gravity_offset + 3], dtype=float
            )
        if estimate_gyro_scale:
            candidate_gyro_scale = gyro_scale + np.asarray(
                delta[gyro_scale_offset : gyro_scale_offset + 3], dtype=float
            )
        if estimate_accel_scale:
            candidate_accel_scale = accel_scale + np.asarray(
                delta[accel_scale_offset : accel_scale_offset + 3], dtype=float
            )
        if estimate_imu_rot:
            candidate_imu_rotation = rotation_matrix_to_rotation_vector(
                rotation_vector_to_matrix(imu_rotation_vector)
                @ rotation_vector_to_matrix(
                    np.asarray(
                        delta[imu_rotation_offset : imu_rotation_offset + 3], dtype=float
                    )
                )
            )
        candidate_objective = _objective(
            problem,
            point_rows,
            plane_rows,
            pose_rows,
            imu_rows,
            lever_rows,
            candidate,
            candidate_bias,
            candidate_lever,
            candidate_clock,
            candidate_accel,
            candidate_gravity,
            candidate_gyro_scale,
            candidate_accel_scale,
            imu_rotation=_imu_rotation_matrix(candidate_imu_rotation),
        )
        knot_delta = np.asarray(delta[: 6 * knot_count], dtype=float).reshape(-1, 6)
        step_translation = float(np.max(np.abs(knot_delta[:, :3]))) if knot_delta.size else 0.0
        step_rotation_rad = float(np.max(np.abs(knot_delta[:, 3:]))) if knot_delta.size else 0.0
        extra = delta[6 * knot_count :]
        if extra.size:
            step_rotation_rad = max(step_rotation_rad, float(np.max(np.abs(extra))))
        step_rotation = math.degrees(step_rotation_rad)
        if candidate_objective < objective:
            objective = candidate_objective
            knots = candidate
            gyro_bias = candidate_bias
            lever_arm = candidate_lever
            clock_offset = candidate_clock
            accel_bias = candidate_accel
            gravity = candidate_gravity
            gyro_scale = candidate_gyro_scale
            accel_scale = candidate_accel_scale
            imu_rotation_vector = candidate_imu_rotation
            if objective < best_objective:
                best_objective = objective
                best_knots = list(knots)
                best_bias = np.array(gyro_bias, copy=True)
                best_lever = np.array(lever_arm, copy=True)
                best_clock = clock_offset
                best_accel = np.array(accel_bias, copy=True)
                best_gravity = np.array(gravity, copy=True)
                best_gyro_scale = np.array(gyro_scale, copy=True)
                best_accel_scale = np.array(accel_scale, copy=True)
                best_imu_rotation = np.array(imu_rotation_vector, copy=True)
            max_step_translation = max(max_step_translation, step_translation)
            max_step_rotation_deg = max(max_step_rotation_deg, step_rotation)
            damping = max(damping * settings.damping_scale_down, 1.0e-12)
            if (
                step_translation < settings.convergence_step_norm
                and step_rotation_rad < settings.convergence_step_norm
            ):
                status = "converged"
                break
        else:
            damping *= settings.damping_scale_up
            if damping > 1.0e12:
                break

    if status != "converged" and best_objective < objective:
        knots = best_knots
        gyro_bias = best_bias
        lever_arm = best_lever
        clock_offset = best_clock
        accel_bias = best_accel
        gravity = best_gravity
        gyro_scale = best_gyro_scale
        accel_scale = best_accel_scale
        imu_rotation_vector = best_imu_rotation
        objective = best_objective
    calibration_names: tuple[str, ...] = ()
    calibration_covariance: tuple[tuple[float, ...], ...] | None = None
    residual_variance: float | None = None
    layout = _calibration_layout(problem)
    if status != "singular_system":
        _final_blocks, final_residuals, final_weights, final_jacobian = _linearize(
            knots,
            gyro_bias,
            lever_arm,
            clock_offset,
            accel_bias,
            gravity,
            gyro_scale,
            accel_scale,
            imu_rotation_vector,
        )
        final_weighted_jacobian = final_jacobian.multiply(final_weights[:, None])
        final_rhs = final_weighted_jacobian.T @ (final_weights * final_residuals)
        gradient_norm = float(np.max(np.abs(final_rhs)))
        if layout:
            covariance, residual_variance = _marginal_calibration_covariance(
                final_jacobian, final_residuals, final_weights, knot_count
            )
            calibration_names = tuple(
                f"{name}[{axis}]" for name, width in layout for axis in range(width)
            )
            calibration_covariance = tuple(
                tuple(float(value) for value in row) for row in covariance
            )
    final_point_rmse = _point_rmse(problem, point_rows, knots)
    final_point_to_plane_rmse = _point_to_plane_rmse(plane_rows, timestamps, knots)
    final_pose_rmse = _pose_rmse(problem, pose_rows, knots)
    final_imu_rmse = imu_preintegration_rmse(
        tuple(imu_rows),
        timestamps,
        knots,
        gyro_bias,
        clock_offset,
        gyro_scale,
        imu_rotation_vector_rad=imu_rotation_vector,
    )
    final_lever_rmse = imu_lever_arm_rmse(
        tuple(lever_rows),
        timestamps,
        knots,
        lever_arm,
        clock_offset,
        accel_bias,
        gravity,
        accel_scale,
        imu_rotation_vector_rad=imu_rotation_vector,
    )
    return ContinuousTimeTrajectoryFitResult(
        status=status,
        iterations=iterations,
        knot_poses=tuple(knots),
        initial_knot_poses=problem.initial_knot_poses,
        final_objective=objective,
        final_point_rmse=final_point_rmse,
        final_point_to_plane_rmse=final_point_to_plane_rmse,
        final_pose_rmse=final_pose_rmse,
        final_imu_rotation_rmse_rad=final_imu_rmse,
        final_lever_arm_rmse_m_s2=final_lever_rmse,
        gyro_bias_rad_s=(
            (float(gyro_bias[0]), float(gyro_bias[1]), float(gyro_bias[2]))
            if imu_rows
            else None
        ),
        lever_arm_body_m=(
            (float(lever_arm[0]), float(lever_arm[1]), float(lever_arm[2]))
            if lever_rows
            else None
        ),
        imu_clock_offset_sec=(
            float(clock_offset) if imu_rows or lever_rows else None
        ),
        accel_bias_body_m_s2=(
            (float(accel_bias[0]), float(accel_bias[1]), float(accel_bias[2]))
            if lever_rows
            else None
        ),
        gravity_world_m_s2=(
            (float(gravity[0]), float(gravity[1]), float(gravity[2]))
            if lever_rows
            else None
        ),
        gyro_scale=(
            (float(gyro_scale[0]), float(gyro_scale[1]), float(gyro_scale[2]))
            if imu_rows
            else None
        ),
        accel_scale=(
            (float(accel_scale[0]), float(accel_scale[1]), float(accel_scale[2]))
            if lever_rows
            else None
        ),
        max_step_translation_m=max_step_translation,
        max_step_rotation_deg=max_step_rotation_deg,
        gradient_norm=gradient_norm,
        nonzero_jacobian_blocks=len(blocks),
        imu_rotation_vector_rad=(
            (
                float(imu_rotation_vector[0]),
                float(imu_rotation_vector[1]),
                float(imu_rotation_vector[2]),
            )
            if imu_rows or lever_rows
            else None
        ),
        calibration_parameter_names=calibration_names,
        calibration_covariance=calibration_covariance,
        residual_variance=residual_variance,
    )
