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

estimateGravityRotationAndPoseScale

Calcule la rotación de la gravedad y la escala de pose utilizando mediciones IMU y optimización del gráfico de factores

Desde R2023a

Sintaxis

[gRot,scale,info] = estimateGravityRotationAndPoseScale(poses,gyroscopeReadings,accelerometerReadings,Name=Value)

Descripción

La función estimateGravityRotationAndPoseScale estima la rotación de la gravedad y la escala de pose que ayuda a transformar las poses de entrada al marco de referencia de navegación local de IMU utilizando mediciones de IMU y optimización del gráfico de factores. La rotación de la gravedad transforma el vector de gravedad del marco de referencia de navegación local de IMU al marco de referencia de pose. La escala de pose trae las poses de entrada a la escala métrica, similar a las mediciones de IMU.

Las mediciones del acelerómetro contienen una aceleración de la gravedad constante que no contribuye al movimiento. Debe eliminar esto de las mediciones para una fusión precisa con otros datos del sensor. Es posible que el marco de referencia de pose de entrada no siempre coincida con el marco de referencia de navegación local de IMU, Noreste-Abajo (NED) o Este-Norte-Arriba (ENU) en el que se conoce la dirección de la gravedad. Por lo tanto, es necesario transformar las poses de entrada al marco de navegación local para eliminar el efecto de gravedad conocido. La rotación estimada ayuda a transformar el marco de referencia de pose de entrada al marco de referencia de navegación local de IMU.

Las estimaciones de la estructura basada en sensores de cámara monocular a partir de movimiento (SfM) se posan en una escala desconocida diferente de las mediciones métricas obtenidas por una IMU. Las lecturas del acelerómetro ayudan a estimar el factor de escala para llevar las poses de entrada a una escala métrica similar a las mediciones de IMU.

[gRot,scale,info] = estimateGravityRotationAndPoseScale(poses,gyroscopeReadings,accelerometerReadings,Name=Value) estima la rotación requerida 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. La función también estima la escala, por lo que las poses de entrada en una escala desconocida se pueden convertir a unidades métricas similares a las de las mediciones de IMU.

Nota

Las poses de entrada deben estar en el marco de referencia inicial de IMU a menos que especifique el argumento nombre-valor SensorTransform , entonces las poses pueden estar en un marco diferente.

Ejemplos

Calcule la rotación y dirección de la gravedad y la escala de pose mediante mediciones IMU y optimización del gráfico de factores

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)

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
% % gravity vector in pose reference frame.
% gravityMagnitude = 9.81;
% gravity = gravityDirection*gravityMagnitude

Argumentos de entrada

poses — Poses de cámara

N-por-7 matriz | arreglo de objetos se3 | tabla | arreglo de rigidtform3d objetos

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:

• N-por-7 matriz, 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 pose 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) .

gyroscopeReadings — Lecturas de giroscopio entre vistas o poses de cámara consecutivas

(N-1) arreglo de celdas de elementos de M-por-3 matrices

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

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

Tipos de datos: cell

accelerometerReadings — Lecturas del acelerómetro entre vistas o poses consecutivas de la cámara.

(N-1) arreglo de celdas de elementos de M-por-3 matrices

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

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 por gravedad basándose en una IMU.

IMUParameters — Parámetros de la IMU

factorIMUParameters() (predeterminado) | factorIMUParameters objeto

Parámetros de IMU, especificados como un objeto factorIMUParameters .

Ejemplo: IMUParameters=factorIMUParameters(SampleRate=100)

SolverOptions — Opciones de solución

factorGraphSolverOptions() (predeterminado) | factorGraphSolverOptions objeto

Opciones de resolución, especificadas como un objeto factorGraphSolverOptions .

Ejemplo: SolverOptions=factorGraphSolverOptions(MaxIterations=50)

SensorTransform — Transformación que consiste en traslación y rotación 3-D para transformar la pose o el punto en el marco de referencia de la pose de entrada al marco IMU inicial.

se3() (predeterminado) | se3 objeto

Transformación que consiste en traslación y rotación 3-D para transformar una cantidad como una pose o un punto en el marco de referencia de pose de entrada al marco de referencia inicial del sensor IMU, especificado como un objeto se3 .

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 marco de referencia inicial del sensor tiene la primera posición del sensor en su origen.

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

Argumentos de salida

gRot — Rotación por gravedad

Matriz de 3 por 3 | se3 objeto | rigidtform3d objeto

Rotación por gravedad, devuelta como una matriz de 3 por 3, objeto se3 o 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.

scale — Multiplicador por el cual escalar las poses de entrada

escalar numérico

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

info — Información de la solución de optimización del gráfico de factores

estructura

Información de la solución de optimización del gráfico de factores, devuelta como una estructura. Los campos de la estructura son:

Campo	Descripción
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 reduce el coste después de la optimización. 1 : Solver no pudo encontrar una solución que cumpla con el criterio de convergencia después de ejecutar el número máximo 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).

Utilice esta información para comprobar si la optimización requerida para la alineación ha convergido o no.

Tipos de datos: struct

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 R2023a

Consulte también

factorIMU | factorIMUParameters | estimateGravityRotation