Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

estimateGravityRotation

Estimar la rotación de la gravedad mediante la optimización del gráfico de factores

Desde R2023a

contraer todo en la página

    Sintaxis

    [gRot,info] = estimateGravityRotation(poses,gyroscopeReadings,accelerometerReadings)
    [gRot,info] = estimateGravityRotation(___,Name=Value)

    Descripción

    [gRot,info] = estimateGravityRotation(poses,gyroscopeReadings,accelerometerReadings) estima la rotación de la gravedad, utilizando la optimización del gráfico de factores, que ayuda a transformar la entrada poses al marco de referencia de navegación local de una unidad de medición inercial (IMU). gyroscopeReadings y accelerometerReadings son las lecturas del giroscopio y las lecturas del acelerómetro de la IMU, respectivamente. La función también devuelve info, que contiene las métricas de validación para la estimación, así como estimaciones de velocidad y sesgo para usar para actualizar el gráfico de factores. Para obtener más información sobre la rotación gravitacional y la validación de las estimaciones, consulte Rotación de gravedad y escala de pose.

    Nota

    Las poses de entrada deben estar en el marco de referencia IMU inicial, a menos que especifique el argumento de nombre-valor SensorTransform. Luego las poses pueden estar en un marco diferente. Esta función asume que las poses de entrada están en la escala del mundo real. Si sus poses de entrada están en una escala desconocida, puede estimar su escala de pose y rotación de gravedad utilizando la función estimateGravityRotationAndPoseScale.

    ejemplo

    [gRot,info] = estimateGravityRotation(___,Name=Value) especifica opciones que utilizan uno o más argumentos de nombre-valor para mejorar el proceso de estimación, además de los argumentos de entrada de la sintaxis anterior. Por ejemplo, FixPoses=true especifica que la función debe tratar las poses de entrada como parámetros fijos durante el proceso de optimización de la estimación.

    ejemplo

    Ejemplos

    contraer todo

    Abrir script en vivo

    Especifique poses de entrada en el primer marco de referencia de pose de la cámara.

    poses = [0.1 0 0 0.7071 0 0 0.7071; ...
         0.1 0.4755 -0.1545 0.7071 0 0 0.7071];

    Especifique 10 lecturas de giroscopio y acelerómetro entre fotogramas de cámara consecutivos.

    accelReadings = repmat([97.9887 -3.0315 -22.0285],10,1);
gyroReadings = zeros(10,3);

    Especifique los parámetros de la IMU.

    params = factorIMUParameters(SampleRate=100, ...
                             ReferenceFrame="NED");

    Especifique una transformación que consista en traslación y rotación 3D para transformar las poses de entrada desde el marco de referencia de pose inicial de la cámara al marco de referencia de pose inicial de IMU. El marco de referencia del sensor inicial tiene la primera posición del sensor en el origen del marco de referencia del sensor.

    sensorTransform = se3(eul2rotm([-pi/2 0 0]),[0 0.1 0]);

    Especifique las opciones del solver de gráficos de factores.

    opts = factorGraphSolverOptions(MaxIterations=50);

    Calcule la rotación de la gravedad utilizando mediciones de IMU entre estimaciones de pose de cámara.

    [gRot,solutionInfo] = estimateGravityRotation(poses, ...
                      {gyroReadings},{accelReadings}, ...
                      IMUParameters=params, ...
                      SensorTransform=sensorTransform, ...
                      SolverOptions=opts)
    gRot = 3×3

    0.0058   -0.6775   -0.7355
    0.1023    0.7320   -0.6736
    0.9947   -0.0713    0.0735


    solutionInfo = struct with fields:
                    Status: SUCCESS
         TranslationErrors: [0.0088 0.0092 5.9933e-04]
            RotationErrors: [0.0092 0.0151 0.0481]
    VelocityInIMUReference: [2×3 double]
                      Bias: [2×6 double]
        PoseToIMUTransform: [1×1 se3]
       PosesInIMUReference: [2×7 double]
               InitialCost: 2.3123e+03
                 FinalCost: 19.0067
              FixedNodeIDs: [0 2 3 6]
          IsSolutionUsable: 1
        NumSuccessfulSteps: 23
      NumUnsuccessfulSteps: 18
          OptimizedNodeIDs: [1 4 5 7 8]
           TerminationType: 0
                 TotalTime: 0.0029

    Utilice la rotación por gravedad para transformar el vector de gravedad desde el marco de navegación local al marco de referencia inicial de la pose de la cámara.

    % Gravity direction in NED frame is along Z-Axis.
gravityDirectionNED = [0; 0; 1];
% Gravity direction in pose reference frame.
gravityDirection = gRot*gravityDirectionNED
    gravityDirection = 3×1

   -0.7355
   -0.6736
    0.0735


    % Gravity vector in pose reference frame.
gravityMagnitude = 9.81;
gravity = gravityDirection*gravityMagnitude
    gravity = 3×1

   -7.2149
   -6.6076
    0.7214

    Argumentos de entrada

    contraer todo

    Poses de cámara o LiDAR, con componente de traslación de cada pose en metros, estimadas por sistemas estéreo-visuales-inerciales o lidar-inerciales, respectivamente, especificadas como uno de estos tipos de pose:

    • Matriz de N por 7, donde cada fila tiene la forma [x y z qw qx qy qz]. Cada fila define la posición xyz, en metros, y la orientación del cuaternión, [qw qx qy qz].

    • Arreglo de objetos se3.

    • Tabla de poses de cámara devuelta por la función poses (Computer Vision Toolbox) del objeto imageviewset (Computer Vision Toolbox).

    • Arreglo de objetos rigidtform3d (Image Processing Toolbox).

    Lecturas del giroscopio entre vistas de cámara o poses consecutivas, especificadas como un arreglo de celdas de (N-1) elementos de matrices de M por 3, en radianes por segundo. N es el número total de vistas de cámara o poses especificado en poses. M es el número de lecturas del giroscopio entre vistas de cámara consecutivas y las tres columnas de gyroscopeReadings representan las [x y z] mediciones entre las vistas de cámara o poses.

    Tenga en cuenta que puede haber un número diferente de lecturas entre vistas de cámara consecutivas.

    Tipos de datos: cell

    Lecturas del acelerómetro entre vistas de cámara o poses consecutivas, especificadas como un arreglo de celdas de (N-1) elementos de matrices de M por 3, en metros por segundo al cuadrado. N es el número total de vistas de cámara o poses especificado en poses. M es el número de lecturas del acelerómetro entre vistas de cámara consecutivas y las tres columnas de accelerometerReadings representan las [x y z] mediciones entre las vistas de cámara o poses.

    Tenga en cuenta que puede haber un número diferente de lecturas entre vistas de cámara consecutivas.

    Tipos de datos: cell

    Argumentos de par nombre-valor

    contraer todo

    Especifique pares de argumentos opcionales como Name1=Value1,...,NameN=ValueN, donde Name es el nombre del argumento y Value es el valor correspondiente. Los argumentos nombre-valor deben aparecer después de los otros argumentos, pero el orden de los pares no importa.

    Ejemplo: estimateGravityRotation(poses,gyroscopeReadings,accelerometerReadings,IMUParameters=factorIMUParameters(SampleRate=100)) estima la rotación de la gravedad basándose en una IMU.

    Parámetros IMU, especificados como un objeto factorIMUParameters.

    Cada IMU puede tener características diferentes especificadas por sus parámetros IMU. Los valores de los parámetros predeterminados en el objeto factorIMUParameters especifican covarianzas de ruido de identidad, que son significativamente más grandes que las covarianzas IMU típicas en escenarios prácticos. Por lo tanto, estimar y especificar los parámetros de la IMU es importante para realizar una estimación precisa de la rotación de la gravedad. Si se desconocen los parámetros de ruido de la IMU, AccelerometerNoise, AccelerometerBiasNoise, GyroscopeNoise y GyroscopeBiasNoise, utilice el análisis de varianza de Allan de las mediciones de la IMU para estimarlos. Para ver un ejemplo de cómo utilizar la varianza de Allan para determinar los parámetros de ruido, consulte Análisis de ruido del sensor inercial utilizando la varianza de Allan.

    Ejemplo: IMUParameters=factorIMUParameters(SampleRate=100)

    Opciones del solucionador, especificadas como un objeto factorGraphSolverOptions.

    Ejemplo: SolverOptions=factorGraphSolverOptions(MaxIterations=50)

    Transformación del marco de referencia de la pose de entrada al marco de referencia del sensor IMU inicial, especificado como un objeto se3.

    La transformación entre el marco de referencia de la pose de entrada y el marco IMU inicial consiste en una traslación y rotación en 3D. Por ejemplo, si las poses de entrada son poses de la cámara en el marco de referencia inicial del sensor de la cámara, entonces la transformación del sensor gira y traduce una pose o un punto en el marco de referencia inicial del sensor de la cámara al marco de referencia inicial del sensor IMU. El origen del marco de referencia del sensor inicial es la primera pose de ese sensor.

    En algunos casos, la cámara y la IMU pueden estar unidas rígidamente a un marco de referencia base común, y las poses se calculan en el marco de referencia base. En este caso, la transformación del sensor especifica la transformación del marco de referencia base al marco de referencia IMU. Si se desconoce la transformación del sensor, se puede estimar mediante calibración extrínseca. Para ver un ejemplo que muestra el proceso de calibración extrínseca entre una cámara y una IMU para estimar una transformación homogénea SE(3), consulte Estimación de la transformación de cámara a IMU mediante calibración extrínseca.

    Ejemplo: SensorTransform=se3(eul2rotm([-pi/2 0 0]),[0 0.1 0])

    Desde R2025a

    Poses de entrada fijas durante la estimación, especificadas como true o false lógicos. Cuando se establece en true, la función trata las poses de entrada como parámetros fijos durante el proceso de optimización de la estimación. Cuando se establece en false, la función trata las poses de entrada como parámetros ajustables que puede refinar durante la optimización, pero solo dentro de un pequeño vecindario de sus valores especificados. No fijar las poses proporciona al evaluador más libertad y, en general, logra mejores resultados. Si espera que las poses de entrada sean muy precisas, corregirlas es computacionalmente menos costoso y puede generar una mayor precisión.

    Desde R2025a

    Estimación de velocidad inicial para la IMU, especificada como un vector fila de tres elementos. Por defecto, la velocidad inicial es desconocida. Especificar la velocidad inicial ayuda a estimar el sesgo. Puede calcular la velocidad inicial a partir de mediciones GPS.

    Ejemplo: InitialVelocity=[0 0 0] specifies the initial velocity in all directions as 0

    Tipos de datos: double

    Desde R2025a

    Umbral de predicción de pose de IMU, especificado como un vector fila de dos elementos con la forma [rotationErrorThreshold translationErrorThreshold]. Si cualquier error de rotación en el campo RotationErrors o cualquier error de traducción en el campo TranslationErrors de info excede rotationErrorThreshold, translationErrorThreshold respectivamente, el campo Status de la estructura info indica una fallo debido a una mala predicción.

    El error de rotación y el error de traducción miden qué tan bien se alinean las predicciones de la pose de la IMU con las poses de entrada después de la estimación. Este argumento define los errores máximos aceptables de rotación y traslación en radianes y metros respectivamente.

    El error de rotación es la diferencia en la rotación relativa entre un par de poses de entrada sucesivas calculadas directamente a partir de las poses de entrada y utilizando la predicción de poses IMU.

    El error de traducción es la diferencia en la traducción relativa entre un par de poses de entrada sucesivas calculadas directamente a partir de las poses de entrada y utilizando la predicción de poses IMU.

    Durante la estimación, la optimización minimiza estos errores. Los errores más bajos indican una mejor alineación entre las predicciones de la pose de la IMU y las poses de entrada y, por lo tanto, mejores estimaciones. Si espera que las poses de entrada sean precisas, puede especificar umbrales de predicción más pequeños, ya que requieren que las predicciones de poses de IMU se alineen más de cerca con las poses de entrada. Utilice umbrales más grandes cuando espere que las poses de entrada sean menos precisas.

    Tipos de datos: double

    Argumentos de salida

    contraer todo

    Rotación de gravedad, devuelta como una matriz de 3 por 3, un objeto se3 o un objeto rigidtform3d (Image Processing Toolbox) similar al tipo de pose de entrada. Contiene la rotación necesaria para transformar el vector de gravedad desde el marco de referencia de navegación local de IMU (NED o ENU) al marco de referencia de pose de entrada.

    Diagnósticos de estimación y resultados de optimización, devueltos como una estructura. Los campos de la estructura son:

    CampoDescripción
    Status

    Estado de los cheques, devuelto como una enumeración con uno de estos valores.

    Valor de enumeraciónDescripción
    SUCCESS

    Sin fallos.

    FAILURE_BAD_PREDICTION

    El error de predicción está por encima del umbral especificado. Esto indica que uno o más errores de rotación en el campo RotationErrors, o uno o más errores de traducción en el campo TranslationErrors, han excedido el valor rotationErrorThreshold, translationErrorThreshold, respectivamente, del argumento PredictionThreshold.

    FAILURE_BAD_BIAS

    La variación de sesgo con respecto al valor inicial está fuera de los límites calculados a partir de los valores de covarianza de sesgo proporcionados.

    FAILURE_BAD_PREDICTION_BIAS

    El error de predicción está por encima del umbral especificado y la variación del sesgo respecto del valor inicial está fuera de los límites esperados.

    FAILURE_BAD_OPTIMIZATION

    La optimización encontró errores y las estimaciones no se pueden utilizar.

    FAILURE_NO_CONVERGENCE

    La optimización de inicialización no convergió en el número especificado de iteraciones. Aumente el número máximo de iteraciones.

    TranslationErrors

    Errores de traducción en la predicción de la pose de IMU, devueltos como una matriz de (N-1) por 3. Cada fila representa la diferencia en la traducción relativa entre un par de poses de entrada sucesivas calculadas directamente a partir de las poses de entrada y utilizando la predicción de poses IMU, en la forma [x y z]. Un error de traducción menor indica que la traducción relativa predicha utilizando mediciones IMU es muy cercana a la estimación de odometría visual.

    RotationErrors

    Errores de rotación en la predicción de la pose de la IMU, devueltos como una matriz de (N-1) por 3. Cada fila representa la diferencia en la rotación relativa entre un par de poses de entrada sucesivas calculadas directamente a partir de las poses de entrada y utilizando la predicción de poses IMU, en la forma [x y z]. Un error de rotación menor indica que la rotación relativa predicha utilizando mediciones de IMU es muy cercana a la estimación de odometría visual.

    InitialCost

    Coste inicial del problema de mínimos cuadrados no lineales formulado por el gráfico de factores antes de la optimización.

    FinalCost

    Coste final del problema de mínimos cuadrados no lineales formulado por el gráfico de factores después de la optimización.

    Nota

    El coste es la suma de términos de error, conocidos como residuos, donde cada residual es una función de un subconjunto de mediciones de factores.

    NumSuccessfulSteps

    Número de iteraciones en las que el solver reduce el coste. Este valor incluye la iteración de inicialización en 0 además de las iteraciones del minimizador.

    NumUnsuccessfulSteps

    Número de iteraciones en las que la iteración no es numéricamente válida o el solver no reduce el coste.

    TotalTime

    Tiempo total de optimización del solver en segundos.

    TerminationType

    Tipo de terminación como un número entero en el rango [0, 2]:

    • 0 — Solver encontró una solución que cumple con el criterio de convergencia y disminuye el costo después de la optimización.

    • 1 — El solucionador no pudo encontrar una solución que cumpla con el criterio de convergencia después de ejecutar la cantidad máxima de iteraciones.

    • 2 — El solucionador finalizó debido a un error.

    IsSolutionUsable

    La solución se puede utilizar si 1 (true), no se puede utilizar si 0 (false).

    PosesInIMUReference

    Poses optimizadas transformadas en el marco de referencia de navegación IMU utilizando la rotación de gravedad estimada, devueltas en el mismo formato que las poses de entrada.

    VelocityInIMUReference

    Estimaciones de velocidad, devueltas como una matriz numérica de N por 3, que representan las velocidades en el marco de referencia de navegación de la IMU en los tiempos de muestreo de la pose de entrada. Puede especificar que estos valores se puedan establecer como estimaciones iniciales para los nodos de velocidad recién creados en el gráfico del factor inercial visual, utilizando la función nodeState.

    Bias

    Estimaciones de sesgo en los tiempos de muestreo de la pose de entrada, devueltas como una matriz numérica de N por 6. Puede especificar estos valores como estimaciones iniciales para los nodos de sesgo recién creados en el gráfico del factor visual-inercial, utilizando la función nodeState.

    PoseToIMUTransform

    Transformación del marco de referencia de pose de entrada al marco de referencia de navegación IMU, devuelto como un objeto se3. Esta es la inversa de la rotación de la gravedad.

    Tipos de datos: struct

    Más acerca de

    contraer todo

    Referencias

    [1] Campos, Carlos, Richard Elvira, Juan J. Gomez Rodriguez, Jose M. M. Montiel, and Juan D. Tardos. “ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM.” IEEE Transactions on Robotics 37, no. 6 (December 2021): 1874–90. https://doi.org/10.1109/TRO.2021.3075644.

    Capacidades ampliadas

    expandir todo

    Generación de código C/C++
    Genere código C y C++ mediante MATLAB® Coder™.

    Historial de versiones

    Introducido en R2023a

    expandir todo

    Consulte también

    | | | |