Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

estimateGravityRotationAndPoseScale

Estimer la rotation gravitationnelle et l'échelle de pose à l'aide des mesures IMU et de l'optimisation du graphique factoriel

Depuis R2023a

Syntaxe

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

Description

La fonction estimateGravityRotationAndPoseScale estime la rotation gravitationnelle et l'échelle de pose qui aident à transformer les poses d'entrée dans le cadre de référence de navigation local de l'IMU à l'aide des mesures de l'IMU et de l'optimisation du graphique factoriel. La rotation gravitationnelle transforme le vecteur gravité du référentiel de navigation local de l'IMU en référentiel de pose. L'échelle de pose amène les poses d'entrée à l'échelle métrique, similaire aux mesures IMU.

Les mesures de l'accéléromètre contiennent une accélération gravitationnelle constante qui ne contribue pas au mouvement. Vous devez supprimer cela des mesures pour une fusion précise avec d’autres données de capteur. Le cadre de référence de pose d'entrée peut ne pas toujours correspondre au cadre de référence de navigation local de l'IMU, Nord-Est-Bas (NED) ou Est-Nord-Haut (ENU) dans lequel la direction de la gravité est connue. Il est donc nécessaire de transformer les poses d'entrée dans le cadre de navigation local pour supprimer l'effet de gravité connu. La rotation estimée aide à transformer le cadre de référence de pose d'entrée en cadre de référence de navigation locale de l'IMU.

La structure basée sur un capteur de caméra monoculaire à partir du mouvement (SfM) estime les poses à une échelle inconnue différente des mesures métriques obtenues par une IMU. Les lectures de l'accéléromètre aident à estimer le facteur d'échelle pour amener les poses d'entrée à une échelle métrique similaire aux mesures IMU.

[gRot,scale,info] = estimateGravityRotationAndPoseScale(poses,gyroscopeReadings,accelerometerReadings,Name=Value) estime la rotation nécessaire pour transformer le vecteur gravité du cadre de référence de navigation local de l'IMU (NED ou ENU) au cadre de référence de pose d'entrée. La fonction estime également l'échelle, de sorte que les poses d'entrée à une échelle inconnue peuvent être converties en unités métriques similaires à celles des mesures IMU.

Remarque

Les poses d'entrée doivent être dans le cadre de référence IMU initial, sauf si vous spécifiez l'argument nom-valeur SensorTransform , les poses peuvent alors être dans un cadre différent.

Exemples

Estimer la rotation et la direction de la gravité et l'échelle de pose à l'aide des mesures IMU et de l'optimisation du graphique factoriel

Spécifiez les poses d’entrée dans le cadre de référence initial de pose de la caméra.

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

Spécifiez 10 lectures de gyroscope et d’accéléromètre entre des images de caméra consécutives.

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

Spécifiez les paramètres IMU.

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

Spécifiez une transformation composée d’une translation et d’une rotation 3D pour transformer les poses du cadre de référence de pose initial de la caméra au cadre de référence de pose IMU initial.

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

Spécifiez les options du solveur de graphe factoriel.

opts = factorGraphSolverOptions(MaxIterations=50);

Estimez la rotation gravitationnelle et l’échelle de pose à l’aide des mesures IMU entre les images de la caméra.

% [gRot,scale,solutionInfo] = estimateGravityRotationAndPoseScale(poses, ...
%                             {gyroReadings},{accelReadings}, ...
%                             IMUParameters=params, ...
%                             SensorTransform=sensorTransform, ...
%                             SolverOptions=opts)

Utilisez la rotation gravitationnelle pour transformer le vecteur de gravité du cadre de navigation local en cadre de référence de pose de caméra initiale.

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

Arguments d'entrée

poses — Poses de caméra

N-matrice par 7 | tableau d'objets se3 | tableau | tableau d'objets rigidtform3d

Poses de caméra, à une échelle inconnue estimée par une structure à partir d'une caméra monoculaire à partir du mouvement (SfM), spécifiées comme l'un de ces types de pose :

• N-matrice par 7, où chaque ligne est de la forme [x y z qw qx qy qz]. Chaque ligne définit la position du xyz, en mètres, et l'orientation du quaternion, [qw qx qy qz].

• Tableau d'objets se3 .

• Table de pose de caméra renvoyée par la fonction poses (Computer Vision Toolbox) de l'objet imageviewset (Computer Vision Toolbox) .

• Tableau d'objets rigidtform3d (Image Processing Toolbox) .

gyroscopeReadings — Lectures du gyroscope entre des vues ou poses consécutives de la caméra

(N-1) cell array d'éléments de matrices M-par 3

Lectures du gyroscope entre des vues ou des poses de caméra consécutives, spécifiées sous la forme d'un cell array d'éléments ( N-1 ) de matrices M-par-3, en radians par seconde. N est le nombre total de vues ou de poses de caméra spécifié dans poses. M est le nombre de lectures du gyroscope entre les vues consécutives de la caméra et les trois colonnes de gyroscopeReadings représentent le [x y z] mesures entre les vues ou les poses de la caméra.

Notez qu'il peut y avoir un nombre différent de lectures entre des vues consécutives de la caméra.

Types de données : cell

accelerometerReadings — Lectures de l'accéléromètre entre des vues ou poses consécutives de la caméra

(N-1) cell array d'éléments de matrices M-par 3

Lectures de l'accéléromètre entre des vues ou des poses de caméra consécutives, spécifiées sous la forme d'un cell array d'éléments ( N-1 ) de matrices M-par 3, en mètres par seconde carrée. N est le nombre total de vues ou de poses de caméra spécifié dans poses. M est le nombre de lectures d'accéléromètre entre des vues consécutives de la caméra et les trois colonnes de accelerometerReadings représentent le [x y z] mesures entre les vues ou les poses de la caméra.

Notez qu'il peut y avoir un nombre différent de lectures entre des vues consécutives de la caméra.

Types de données : cell

Arguments nom-valeur

Spécifiez des paires d'arguments facultatives sous la forme Name1=Value1,...,NameN=ValueN, où Name est le nom de l'argument et Value est la valeur correspondante. Les arguments nom-valeur doivent apparaître après les autres arguments, mais l'ordre des paires n'a pas d'importance.

Exemple : estimateGravityRotationAndPoseScale(poses,gyroscopeReadings,accelerometerReadings,IMUParameters=factorIMUParameters(SampleRate=100)) estime la rotation gravitationnelle sur la base d'une IMU.

IMUParameters — Paramètres de l'IMU

factorIMUParameters() (par défaut) | factorIMUParameters objet

Paramètres IMU, spécifiés sous la forme d'un objet factorIMUParameters .

Exemple : IMUParameters=factorIMUParameters(SampleRate=100)

SolverOptions — Options du solveur

factorGraphSolverOptions() (par défaut) | factorGraphSolverOptions objet

Options du solveur, spécifiées comme objet factorGraphSolverOptions .

Exemple : SolverOptions=factorGraphSolverOptions(MaxIterations=50)

SensorTransform — Transformation consistant en une translation et une rotation 3D pour transformer la pose ou le point dans le cadre de référence de pose d'entrée en cadre IMU initial

se3() (par défaut) | se3 objet

Transformation consistant en une translation et une rotation 3D pour transformer une quantité telle qu'une pose ou un point dans le cadre de référence de pose d'entrée en cadre de référence initial du capteur IMU, spécifié comme un objet se3 .

Par exemple, si les poses d'entrée sont des poses de caméra dans le cadre de référence initial du capteur de caméra, alors la transformation du capteur tourne et traduit une pose ou un point dans le cadre de référence initial du capteur de caméra vers le cadre de référence initial du capteur IMU. Le référentiel initial du capteur a la toute première pose du capteur à son origine.

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

Arguments de sortie

gRot — Rotation gravitationnelle

Matrice 3x3 | se3 objet | Objet rigidtform3d

Rotation gravitationnelle, renvoyée sous la forme d'une matrice 3 par 3, d'un objet se3 ou d'un objet rigidtform3d (Image Processing Toolbox) similaire au type de pose d'entrée. Il contient la rotation nécessaire pour transformer le vecteur gravité du cadre de référence de navigation local de l'IMU (NED ou ENU) au cadre de référence de pose d'entrée.

scale — Multiplicateur permettant de mettre à l'échelle les poses d'entrée

scalaire numérique

Multiplicateur permettant de mettre à l'échelle les poses d'entrée, renvoyé sous forme de scalaire numérique. Utilisez cette valeur pour mettre à l'échelle les poses d'entrée selon les mêmes unités métriques que les mesures IMU.

Types de données : double

info — Informations sur la solution d'optimisation du graphique factoriel

structure

Informations sur la solution d’optimisation du graphe factoriel, renvoyées sous forme de structure. Les champs de la structure sont :

Champ	Description
InitialCost	Coût initial du problème des moindres carrés non linéaires formulé par le graphe factoriel avant l'optimisation.
FinalCost	Coût final du problème des moindres carrés non linéaires formulé par le graphe factoriel après optimisation. Remarque Le coût est la somme des termes d’erreur, appelés résidus, où chaque résidu est fonction d’un sous-ensemble de mesures factorielles.
NumSuccessfulSteps	Nombre d'itérations au cours desquelles le solveur diminue le coût. Cette valeur inclut l'itération d'initialisation à 0 en plus des itérations de minimisation.
NumUnsuccessfulSteps	Nombre d'itérations dans lesquelles l'itération est numériquement invalide ou le solveur ne diminue pas le coût.
TotalTime	Temps total d’optimisation du solveur en secondes.
TerminationType	Type de terminaison sous forme d'entier compris dans la plage [0, 2] : 0 — Le solveur a trouvé une solution qui répond au critère de convergence et réduit les coûts après optimisation. 1 — Le solveur n'a pas pu trouver de solution répondant au critère de convergence après avoir exécuté le nombre maximal d'itérations. 2 — Le solveur s'est arrêté en raison d'une erreur.
IsSolutionUsable	La solution est utilisable si 1 (true), non utilisable si 0 (false).

Utilisez ces informations pour vérifier si l'optimisation requise pour l'alignement a convergé ou non.

Types de données : struct

Références

[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.

Capacités étendues

Génération de code C/C++
Générez du code C et C++ avec MATLAB® Coder™.

Historique des versions

Introduit dans R2023a

Voir aussi

factorIMU | factorIMUParameters | estimateGravityRotation