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

estimateGravityRotation

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

Desde R2023a

Sintaxis

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

Descripción

La función estimateGravityRotation estima la rotación de gravedad 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.

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 alinear el marco de referencia de pose de entrada y el marco de referencia de navegación local de IMU.

[gRot,info] = estimateGravityRotation(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.

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 mediante mediciones IMU y optimización del gráfico de factores

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)

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 o lidar

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

Poses de cámara o lidar, con unidades métricas similares a las mediciones de IMU estimadas por sistemas estéreo-visual-inercial o lidar-inercial, respectivamente, especificadas 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: estimateGravityRotation(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.

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 | estimateGravityRotationAndPoseScale