Main Content

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

estimateGravityRotation

Estimer la rotation gravitationnelle à l’aide des mesures IMU et de l’optimisation du graphique factoriel

Depuis R2023a

    Description

    La fonction estimateGravityRotation estime la rotation gravitationnelle qui aide à 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.

    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 permet d'aligner le cadre de référence de pose d'entrée et le cadre de référence de navigation locale de l'IMU.

    exemple

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

    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

    réduire tout

    Spécifiez les poses d’entrée dans le premier cadre de référence de pose de 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 d’entrée du cadre de référence de pose initial de la caméra au cadre de référence de pose IMU initial. Le cadre de référence initial du capteur présente une première pose de capteur à l'origine du cadre de référence du capteur.

    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 à l’aide des mesures IMU entre les estimations de pose de caméra.

    % [gRot,solutionInfo] = estimateGravityRotation(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

    réduire tout

    Poses de caméra ou de lidar, avec des unités métriques similaires à celles des mesures IMU estimées respectivement par des systèmes stéréo-visuels-inertiels ou lidar-inertiel, 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) .

    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

    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 : estimateGravityRotation(poses,gyroscopeReadings,accelerometerReadings,IMUParameters=factorIMUParameters(SampleRate=100)) estime la rotation gravitationnelle sur la base d'une IMU.

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

    Exemple : IMUParameters=factorIMUParameters(SampleRate=100)

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

    Exemple : SolverOptions=factorGraphSolverOptions(MaxIterations=50)

    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

    réduire tout

    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.

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

    ChampDescription
    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