estimateGravityRotationAndPoseScale
Estimar la rotación de la gravedad y la escala de la pose utilizando la optimización del gráfico de factores
Desde R2023a
Sintaxis
Descripción
[ estima la rotación de gravedad y la escala de pose de las poses de entrada utilizando la optimización del gráfico de factores. La rotación de gravedad y la escala de pose le ayudan a transformar las poses de la cámara de entrada al marco de referencia de navegación local de una unidad de medición inercial (IMU). La función toma las poses de la cámara, las lecturas del giroscopio y las lecturas del acelerómetro de la IMU como entrada. Además de la rotación de gravedad y la escala de pose, devuelve gRot,scale,info] = estimateGravityRotationAndPoseScale(poses,gyroscopeReadings,accelerometerReadings)info, que contiene las métricas utilizadas para validar las estimaciones, junto con estimaciones de velocidad y sesgo que puede utilizar para actualizar el gráfico de factores. Para obtener más información sobre la rotación de la gravedad, la escala de la pose 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.
[ 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.gRot,scale,info] = estimateGravityRotationAndPoseScale(___,Name=Value)
Ejemplos
Especifique poses de entrada en el marco de referencia de pose inicial 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 desde el marco de referencia de pose inicial de la cámara al marco de referencia de pose inicial de IMU.
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 y la escala de pose utilizando mediciones IMU entre fotogramas de la cámara.
[gRot,scale,solutionInfo] = estimateGravityRotationAndPoseScale(poses, ... {gyroReadings},{accelReadings}, ... IMUParameters=params, ... SensorTransform=sensorTransform, ... SolverOptions=opts)
gRot = 3×3
0.9804 -0.0656 -0.1856
-0.1765 0.1251 -0.9763
0.0872 0.9900 0.1111
scale = 0.7357
solutionInfo = struct with fields:
Status: FAILURE_NO_CONVERGENCE
TranslationErrors: [0.0117 0.0123 0.0024]
RotationErrors: [0.0126 0.0300 0.0607]
VelocityInIMUReference: [2×3 double]
Bias: [2×6 double]
PoseToIMUTransform: [1×1 se3]
PosesInIMUReference: [2×7 double]
InitialCost: 2.3123e+03
FinalCost: 27.9810
FixedNodeIDs: [2 3 6]
IsSolutionUsable: 1
NumSuccessfulSteps: 30
NumUnsuccessfulSteps: 21
OptimizedNodeIDs: [0 1 4 5 7 8]
TerminationType: 1
TotalTime: 0.0085
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.1856
-0.9763
0.1111
% gravity vector in pose reference frame.
gravityMagnitude = 9.81;
gravity = gravityDirection*gravityMagnitudegravity = 3×1
-1.8211
-9.5777
1.0900
Argumentos de entrada
Poses de cámara, a una escala desconocida estimada mediante estructura a partir de movimiento (SfM) basada en cámara monocular, especificada como uno de estos tipos de pose:
Matriz 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].
Matriz de objetos
se3.Tabla de poses de cámara devuelta por la función
poses(Computer Vision Toolbox) del objetoimageviewset(Computer Vision Toolbox).Matriz de objetos
rigidtform3d(Image Processing Toolbox).
Se sugiere establecer la profundidad media de las poses en 1. Esto acerca las poses a la escala métrica de las mediciones de IMU e inicializa el problema de optimización de la estimación de escala cerca de la escala de ground-truth .
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
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: estimateGravityRotationAndPoseScale(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 estimar con precisión la rotación de la gravedad y la escala de la pose. Si se desconocen los parámetros de ruido de IMU, AccelerometerNoise, AccelerometerBiasNoise, GyroscopeNoise y GyroscopeBiasNoise, utilice el análisis de varianza de Allan de las mediciones de 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
Valor de escala mínimo válido, especificado como un escalar numérico. double. Cuando scale es menor que este valor, el campo Status de info indica fallo debido a mala escala. Los valores de escala bajos pueden indicar que la estimación no es exitosa debido a que el ruido en las mediciones de IMU domina las mediciones de IMU reales.
El parámetro ScaleThreshold es especialmente útil cuando los puntos del mapa estimados mediante odometría visual monocular en la referencia de pose de entrada tienen una profundidad de medio de 1 metro. La profundidad media es la distancia media de todos los puntos de referencia triangulados alrededor de la cámara. En tales casos, el umbral de escala representa la profundidad mediana mínima permitida. Por ejemplo, si la profundidad media estimada de los puntos del mapa estimados en la referencia de pose de entrada es de 1 metro, mientras que la profundidad media de la ground-truth es de 10 metros, la escala de pose de la ground-truth es 10.
Un escenario en el que es útil tener una profundidad media de 1 metro es si se determina el umbral de escala en función de las limitaciones de la distancia focal. Normalmente, la profundidad media de la escena no puede ser inferior a 1 mm debido a limitaciones de la distancia focal, ya que la cámara no puede observar con precisión ningún punto de referencia que se encuentre a 1 mm de su campo de visión. Por tanto, la profundidad media mínima permitida es de 1 mm. En este caso, tener la profundidad media en 1 puede ser útil, ya que permite establecer el umbral de escala en 1 mm, filtrando cualquier estimación de escala por debajo de este valor. Téngase en cuenta que el ejemplo de una profundidad mediana mínima permitida de 1 mm es puramente ilustrativo. En la práctica, se puede determinar empíricamente una profundidad media mínima permitida más realista basándose en los datos recopilados.
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
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.
Multiplicador por el cual escalar las poses de entrada, devuelto como un escalar numérico. Utilice este valor para escalar las poses de entrada a las mismas unidades métricas que las medidas de la IMU.
Tipos de datos: double
Diagnósticos de estimación y resultados de optimización, devueltos como una estructura. Los campos de la estructura son:
| Campo | Descripción | ||||||||||||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
Status | Estado de los cheques, devuelto como una enumeración con uno de estos valores.
| ||||||||||||||||||||||
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]:
| ||||||||||||||||||||||
IsSolutionUsable | La solución se puede utilizar si | ||||||||||||||||||||||
PosesInIMUReference | Poses optimizadas transformadas en el marco de referencia de navegación IMU utilizando la rotación de gravedad estimada y la escala de pose, devueltas en el mismo formato que las poses de entrada. | ||||||||||||||||||||||
VelocityInIMUReference | Estimaciones de velocidad, devueltas como una matriz numérica 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 | ||||||||||||||||||||||
Bias | Estimaciones de sesgo en los tiempos de muestreo de la pose de entrada, devueltas como una matriz numérica 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 | ||||||||||||||||||||||
PoseToIMUTransform | Transformación del marco de referencia de pose de entrada al marco de referencia de navegación IMU, devuelto como un objeto |
Tipos de datos: struct
Más acerca de
Para mejorar la precisión de la estimación de la postura mediante odometría visual, la odometría visual-inercial fusiona las mediciones IMU y visuales mediante un gráfico de factores. Para que esta fusión funcione, ambas medidas deben estar en un marco de coordenadas común. Las mediciones del acelerómetro incluyen una aceleración de gravedad constante, que no contribuye al movimiento y debe eliminarse para una integración precisa con otros datos del sensor.
Para eliminar la aceleración de la gravedad, debes conocer la dirección de la gravedad. Como tal, las mediciones IMU comúnmente utilizan los marcos de referencia de navegación local noreste-abajo (NED) y este-norte-arriba (ENU), ya que la dirección de la gravedad se conoce en estos marcos. Sin embargo, es posible que el marco de referencia de la pose de entrada no esté alineado con el marco de referencia de navegación local de la IMU. En estos casos, es necesario transformar las poses de entrada al marco de referencia de navegación local para eliminar los efectos de la gravedad.
Porque el proceso Estructura a partir del movimiento (SfM) basado en el sensor de la cámara monocular estima las poses en una escala desconocida que difiere de la escala métrica de las mediciones de IMU. Por lo tanto, para transformar las poses de entrada al marco de referencia de navegación local de la IMU se necesita un multiplicador escalar para ajustar la escala de las poses de entrada a una escala métrica consistente con las mediciones de la IMU.
La rotación de gravedad transforma el vector de gravedad del marco de referencia de navegación local de la IMU al marco de referencia de pose de entrada. Como tal, puede utilizar la inversa de la rotación de gravedad estimada de la función estimateGravityRotationAndPoseScale para ayudar a transformar las poses de entrada al marco de referencia de navegación local de la IMU.
Escala de pose es el multiplicador escalar para transformar las poses de entrada a una escala métrica similar a las mediciones IMU. La función estimateGravityRotationAndPoseScale estima esta escala de pose utilizando lecturas del acelerómetro.
La precisión de los parámetros estimados por estimateGravityRotationAndPoseScale depende de la precisión de las estimaciones de la pose de entrada, los parámetros de ruido de la IMU y la transformación del sensor. estimateGravityRotationAndPoseScale le permite especificar umbrales de escala y predicción como argumentos de nombre-valor, y devuelve información de depuración en la estructura info para validar las estimaciones. Sin embargo, pasar esta validación no siempre garantiza una estimación exitosa. Para aumentar la confianza en los resultados, realice la estimación varias veces y evalúe la varianza entre las estimaciones. Si la variación entre estas estimaciones es baja, entonces puede confiar en las estimaciones de los parámetros.
Si los parámetros estimados no pasan la validación, considere las siguientes estrategias para mejorar la precisión:
Modifique los argumentos de entrada
FixPoses,InitialVelocityySensorTransform. Para conocer los posibles valores de estos argumentos de entrada y sus efectos, consulte las respectivas descripciones de sus argumentos.Utilice parámetros IMU no predeterminados. Los parámetros predeterminados especifican covarianzas de ruido de identidad, que son significativamente más grandes que las covarianzas IMU típicas en escenarios prácticos.
Si se han adquirido previamente datos de trayectoria, seleccione poses de entrada de la trayectoria de modo que los cuadros correspondientes de las poses tengan una aceleración distinta de cero entre ellos a lo largo de todos los ejes.
Si aún no se han adquirido datos de la trayectoria, recopile datos de la cámara y del sensor IMU al inicio de la navegación, garantizando una velocidad angular moderada y una aceleración a lo largo de todos los ejes posibles.
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
Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.
Historial de versiones
Introducido en R2023aAhora puede especificar los siguientes nuevos argumentos de nombre-valor para mejorar las estimaciones.
FixPoses— Mejora la precisión de la estimación al especificar si las poses de entrada son fijas o no fijas durante la estimación.InitialVelocity— Calcula el sesgo con precisión especificando la velocidad inicial de la IMU.PredictionThreshold— Filtra estimaciones no confiables especificando un valor de umbral de predicción de pose de IMU.ScaleThreshold— Filtra las estimaciones de escala por debajo del valor de escala especificado.
La estructura de salida info ahora incluye estos campos adicionales:
Status— Indica la validez o el motivo del fallo de la estimación.TranslationErrosyRotationErrors— Indican la diferencia en la traslación y rotación estimadas, respectivamente, entre la estimación de odometría visual y la estimación basada en la medición de IMU.PoseToIMUTransform— Transformación del marco de referencia de pose de entrada al marco de referencia de navegación IMU.PosesInIMUReference— Poses optimizadas en el marco de referencia de navegación IMU.VelocityInIMUReferenceyBias— Estimaciones de velocidad en el marco de referencia de navegación IMU y estimaciones de sesgo, respectivamente.
Consulte también
factorIMU | factorIMUParameters | estimateGravityRotation | factorGraphSolverOptions | factorGraph
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Seleccione un país/idioma
Seleccione un país/idioma para obtener contenido traducido, si está disponible, y ver eventos y ofertas de productos y servicios locales. Según su ubicación geográfica, recomendamos que seleccione: .
También puede seleccionar uno de estos países/idiomas:
Cómo obtener el mejor rendimiento
Seleccione China (en idioma chino o inglés) para obtener el mejor rendimiento. Los sitios web de otros países no están optimizados para ser accedidos desde su ubicación geográfica.
América
- América Latina (Español)
- Canada (English)
- United States (English)
Europa
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)