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, 1e-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 > 1000000000000.0:
                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 layout and 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)
        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)
    _report_blocks, _report_r, _report_w, _report_j = _linearize(knots, gyro_bias, lever_arm, clock_offset, accel_bias, gravity, gyro_scale, accel_scale, imu_rotation_vector)
    _report_wj = _report_j.multiply(_report_w[:, None])
    gradient_norm = float(np.max(np.abs(_report_wj.T @ (_report_w * _report_r))))
    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)
